JP3629142B2 - Control device for legged mobile robot - Google Patents

Control device for legged mobile robot Download PDF

Info

Publication number
JP3629142B2
JP3629142B2 JP12523298A JP12523298A JP3629142B2 JP 3629142 B2 JP3629142 B2 JP 3629142B2 JP 12523298 A JP12523298 A JP 12523298A JP 12523298 A JP12523298 A JP 12523298A JP 3629142 B2 JP3629142 B2 JP 3629142B2
Authority
JP
Japan
Prior art keywords
foot
reaction force
floor reaction
target
posture
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Lifetime
Application number
JP12523298A
Other languages
Japanese (ja)
Other versions
JPH11300660A (en
Inventor
透 竹中
忠明 長谷川
隆志 松本
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Honda Motor Co Ltd
Original Assignee
Honda Motor Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Honda Motor Co Ltd filed Critical Honda Motor Co Ltd
Priority to JP12523298A priority Critical patent/JP3629142B2/en
Priority to PCT/JP1999/002075 priority patent/WO1999054095A1/en
Priority to US09/673,629 priority patent/US6289265B1/en
Priority to DE69943148T priority patent/DE69943148D1/en
Priority to EP99913713A priority patent/EP1120203B1/en
Publication of JPH11300660A publication Critical patent/JPH11300660A/en
Application granted granted Critical
Publication of JP3629142B2 publication Critical patent/JP3629142B2/en
Anticipated expiration legal-status Critical
Expired - Lifetime legal-status Critical Current

Links

Images

Landscapes

  • Control Of Position Or Direction (AREA)
  • Manipulator (AREA)

Description

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

Figure 0003629142
【００９４】
このように求めた目標各足平床反力は連続的に変化するので、衝撃の少ない歩行を実現するために適している。尚、上記の詳細は本出願人が別途提案した技術（特開平６−７９６５７号）に記述されている。
【００９５】
図４の説明に戻ると、この装置は姿勢安定化制御演算部を備え、姿勢安定化制御演算部はロボットのセンサ情報に基づいてロボットの状態を推定し、補償全床反力を算出する。即ち、実際にロボットが歩行あるいは直立しているときなどには後述する変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたとしても、ロボットの位置・姿勢は必ずしも望ましい位置・姿勢にならない。
【００９６】
ロボットの姿勢を長期的に安定化させるためには、ロボットを望ましい位置・姿勢に復元させるために必要な力とモーメントを求め、これを目標全床反力中心点（目標ＺＭＰ）を作用点として付加的に発生させる必要がある。この付加的な力とモーメントを補償全床反力という。また、補償全床反力のモーメント成分を補償全床反力モーメントという。
【００９７】
尚、脚式移動ロボットの目標歩容が床反力以外の反力を環境から受けるように想定し、それを例えば、目標対象物反力と称し、先に述べた目標ＺＭＰの定義を次のように拡張しても良い。即ち、目標運動パターンによって発生する慣性力と重力と目標対象物反力の合力を動力学的に求め、それが床面上のある点に作用するモーメントが、鉛直軸まわりの成分を除いて零であるならば、その点を改めて目標ＺＭＰとするようにしても良い。
【００９８】
もし、ロボット１が完全剛体であって、変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたと仮定すると、足平のばね機構３２および足底弾性体３４のたわみによって生じるロボット全体の位置・姿勢の摂動的な運動は、以下の６自由度に分解できる。
【００９９】
モード１）目標全床反力中心点（目標ＺＭＰ）を中心とした前後軸まわり回転（即ち、左右傾き）
モード２）目標全床反力中心点（目標ＺＭＰ）を中心とした左右軸まわり回転（即ち、前後傾き）
モード３）目標全床反力中心点（目標ＺＭＰ）を中心とした鉛直軸まわり回転（即ち、スピン）
モード４）前後平行移動揺れ
モード５）左右平行移動揺れ
モード６）上下平行移動揺れ
【０１００】
この内で、モード４とモード５は、足平のばね機構３２および弾性体３４が前後左右方向のせん断力を受けて撓むことによって発生するものである。ばね機構３２および足底弾性体３４は剪断方向の剛性が高いように製作するので、この揺れは極めて少なく、歩行に及ぼす悪影響はほとんどない。
【０１０１】
先に提案した技術では主としてモード１とモード２を扱っていたが、この発明に係る実施の形態では、それに加えてモード６も扱う。尚、理解の便宜のため、重複するが、先の提案技術を説明する。
【０１０２】
モード１を制御するための操作量は、補償全床反力の前後軸（Ｘ軸）まわりモーメント成分である。モード２を制御するための操作量は、補償全床反力の左右軸（Ｙ軸）まわりモーメント成分である。従って、補償全床反力の成分の内、前後軸方向モーメント成分と左右軸方向モーメント成分だけを求めれば良い。他の成分は、この実施の形態では用いないので０で良い。
【０１０３】
尚、以降は次の定義に従う。即ち、補償全床反力のモーメント成分を補償全床反力モーメントＭｄｍｄ（詳しくは目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントＭｄｍｄ）という。図５に示す如く、ロボットの前方向をＸ軸、左横方向をＹ軸、上方向をＺ軸にとり、第１足平の足首直下の床面上の点を原点とした座標系を支持脚座標系と呼び、断らない限り、位置、力およびモーメントはこの座標系で表現されるものとする。また、ＭｄｍｄのＸ成分をＭｄｍｄｘ、Ｙ成分をＭｄｍｄｙ、Ｚ成分をＭｄｍｄｚと記述する。上体２４の傾斜偏差（即ち、実上体傾斜−目標上体傾斜）θｅｒｒ のＸ成分をθｅｒｒｘ， Ｙ成分をθｅｒｒｙ、これらの時間微分値を（ｄθｅｒｒｘ ／ ｄｔ），（ｄθｅｒｒｙ ／ ｄｔ）と記述する。
【０１０４】
ＭｄｍｄｘおよびＭｄｍｄｙは、例えば次式の制御則によって決定される。
Figure 0003629142
ここで、Ｋｔｈｘ，Ｋｔｈｙ，ＫｗｘおよびＫｗｙは、上体傾斜安定化制御ゲインである。
【０１０５】
後述する複合コンプライアンス動作決定部は、目標全床反力と補償全床反力の合力に実全床反力を一致させようと働く。
【０１０６】
図４の説明に戻ると、この装置は実各足平床反力検出器を備え、実各足平床反力検出器は、６軸力センサ４４によって実各足平床反力（その合力が実全床反力）を検出する。更に、関節のエンコーダによって検出される実変位（あるいは変位指令）に基づき、上体に固定された座標系に対する各足平の相対位置・姿勢を算出し、それによって６軸力センサ４４の検出値を座標変換し、上体に固定された座標系で表現された実各足平床反力を算出した後、更に、支持脚座標系に変換する。
【０１０７】
この装置はロボット幾何学モデル（逆キネマティクス演算部）を備え、ロボット幾何学モデルは、上***置・姿勢と足平位置・姿勢を入力されると、それらを満足する各関節変位を算出する。この実施の形態におけるロボット１のような１脚あたりの関節自由度が６である場合には、各関節変位は一義的に求まる。
【０１０８】
この実施の形態では逆キネマティクスの解の式を直接的に求めておき、式に上***置・姿勢と足平位置・姿勢を代入するだけで各関節変位を得るようにした。即ち、ロボット幾何学モデルは、目標上***置・姿勢と複合コンプライアンス動作決定部で修正された修正目標足平位置・姿勢軌道（機構変形補償入り修正目標足平位置・姿勢軌道）を入力し、それらから１２個の関節（１０Ｒ（Ｌ）など）の関節変位指令（値）を算出する。尚、ヤコビアンによって関節角変位を得ても良い。
【０１０９】
この装置は変位コントローラ（前記した第２の演算装置８２に同じ）を備え、変位コントローラは、ロボット幾何学モデル（逆キネマティクス演算部）で算出された関節変位指令（値）を目標値としてロボット１の１２個の関節の変位を追従制御する。
【０１１０】
前記した複合コンプライアンス動作決定部は、以下の２つの要求を満足させようと、目標足平位置・姿勢軌道を修正する。
【０１１１】
要求１）ロボットの位置・姿勢制御のために、実全床反力を姿勢安定化制御部が出力する補償全床反力（モーメントＭｄｍｄ）と目標全床反力の合力に追従させる。ロボットの姿勢傾きだけを制御したい場合には、目標全床反力中心点まわりの実全床反力水平方向モーメント成分だけを補償全床反力モーメントＭｄｍｄに追従させる。
【０１１２】
要求２）各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。
【０１１３】
尚、補足すると、通常は実全床反力を補償全床反力と目標全床反力の合力に一致させながら目標各足平床反力中心点まわりの実各足平床反力モーメントを０にすることが、物理的に不可能な場合が多い。従って、要求１）と要求２）は完全に両立させることはできず、ある点で妥協しなくてはならない。
【０１１４】
上記を前提として図１０フロー・チャート（構造化フロー・チャート）を参照してこの装置の動作を説明する。尚、図の左端に該当する処理を行う図４装置の構成要素を示す。
【０１１５】
先ずＳ１０において装置を初期化し、Ｓ１２を経てＳ１４に進み、タイマ割り込みを待機する。タイマ割り込みは５０ｍｓごとになされ、即ち、制御周期は５０ｍｓである
【０１１６】
続いてＳ１６に進んで歩容の切り替わり目、即ち、支持脚の切り替わり目か否か判断し、否定されるときはＳ２２に進むと共に、肯定されるときはＳ１８に進んでタイマｔをイニシャライズし、Ｓ２０に進んで目標歩容パラメータを設定する。前記の如く、歩容パラメータは、運動パラメータと床反力パラメータ（ＺＭＰ軌道パラメータ）から構成される。
【０１１７】
続いてＳ２２に進み、目標歩容の瞬時値を決定する。ここで『瞬時値』は制御周期ごとの値を意味し、目標歩容瞬時値は、目標上***置・姿勢、目標各足平位置・姿勢、および目標ＺＭＰ位置から構成される。尚、ここで『姿勢』はＸ，Ｙ，Ｚ空間における『向き』を意味する。
【０１１８】
続いてＳ２４に進んで目標各足平床反力中心点を求める。これは、目標床反力分配器の説明で述べたように行う。具体的には、図８および図９に示すように設定した目標各足平床反力中心点軌跡の現在時刻ｔにおける値を求めることで行う。
【０１１９】
続いてＳ２６に進んで目標各足平床反力を求める。これは目標床反力分配器の説明で述べた式１を用いて目標各足平床反力を演算することで行う。
【０１２０】
続いてＳ２８に進み、前記した傾斜センサ６０などの出力から上体２４の傾斜などロボット１の状態を検出する。このとき、上体２４に作用する実際のＸ，Ｙ，Ｚ方向の加速度を検出する。前記した課題を解決するため、この実施の形態では特にＺ方向の上体加速度（「Ｇｂｏｄｙｚ」という）を例にとって説明するが、以下の説明はＸ，Ｙ方向についても同様に妥当する。
【０１２１】
続いてＳ３０に進み、ロボット１の状態などから姿勢を安定化するための（目標全床反力中心点（目標ＺＭＰ）まわりの）補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを求める。具体的には、上体傾斜が検出されたとき姿勢安定化を図るために前記した式２に従って補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを演算する。
【０１２２】
続いてＳ３１に進んで上体Ｚ方向加速度制御則に基づいて補償全床反力の力成分を算出するが、これについては後述する。
【０１２３】
続いてＳ３２に進んで実各足平床反力を検出する。これは前記の如く、６軸力センサ４４の出力から検出する。
【０１２４】
続いてＳ３４に進み、両脚補償角θｄｂｖおよび各足平補償角θｎｘ（ｙ）を決定する。これは、前記した複合コンプライアンス動作決定部が行う作業である。
【０１２５】
その複合コンプライアンス動作決定部の作業について説明する。説明の便宜のため、両脚支持期において図１１に示すように第１足平２２Ｒ（Ｌ）と第２足平２２Ｌ（Ｒ）に実各足平床反力が作用している状況と仮定する。
【０１２６】
ここでベクトルＦｎａｃｔは第ｎ足平床反力の力成分を表す。ベクトルＭｎａｃｔは第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎａｃｔの向きは、向きに対して時計回りのモーメントが床から足平に作用していることを表す。
【０１２７】
この瞬間の目標全床反力は、図１２に示すようになっていると仮定する。ちなみに、目標全床反力中心点（目標ＺＭＰ）における目標全床反力モーメントベクトルＭｓｕｍｒｅｆは垂直である（定義により、目標ＺＭＰは目標全床反力モーメントの水平方向成分が０である点であるから）。
【０１２８】
これを式１に従って目標各足平床反力に分配すると、図１３に示すようになる。同図において、ベクトルＦｎｒｅｆは目標第ｎ足平床反力の力成分を表す。ベクトルＭｎｒｅｆは目標第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎｒｅｆの向きの表現は、Ｍｎａｃｔと同様である。
【０１２９】
説明のため、上体姿勢が左後ろに倒れそうな状態を想定する。
【０１３０】
前述の姿勢安定化制御演算部では、ロボット１の上体傾斜偏差検出値θｅｒｒｘ，θｅｒｒｙに基づいて補償全床反力モーメントＭｄｍｄを算出する。この実施の形態では鉛直軸（Ｚ軸）まわりのスピンを制御しないので、補償全床反力モーメントＭｄｍｄの鉛直軸成分は０である。この状態に対応する補償全床反力モーメントＭｄｍｄを図１４に示す。
【０１３１】
姿勢を復元させるためには、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平成分を、目標全床反力モーメントＭｓｕｍｒｅｆと補償全床反力モーメントＭｄｍｄの和の水平成分に追従させれば良い。
【０１３２】
一方、目標全床反力中心点（目標ＺＭＰ）では目標全床反力モーメントＭｓｕｍｒｅｆの水平方向成分は０である。従って、前後左右の姿勢傾きを復元させるためには、目標ＺＭＰまわりの実全床反力モーメントの水平成分を、Ｍｄｍｄの水平成分に追従させれば良い。
【０１３３】
この実施の形態にあっては複合コンプライアンス動作決定部は、以下の要求をできる限り満足するように足平の位置・姿勢を修正する。
要求１）ロボットの姿勢傾斜を安定化制御するために、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平方向（Ｘ，Ｙ軸方向）成分を、補償全床反力モーメントＭｄｍｄの水平方向成分に追従させる。
要求２）各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。
【０１３４】
但し、前述の通り、要求１）と要求２）は、完全に両立させることはできず、ある点で妥協しなくてはならない。
【０１３５】
足平の位置・姿勢の修正は、この実施の形態では次のように行う。
１）目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を含み、かつ水平面と垂直な平面の法線ベクトルＶを求める。Ｖの大きさは１とする。Ｖを図１５に示す。
【０１３６】
２）目標第１足平床反力中心点Ｑ１の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに、ある回転角θｄｂｖだけ回転移動する。移動した後の点をＱ１’とする。同様に、目標第２足平床反力中心点Ｑ２の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに回転角θｄｂｖだけ回転移動する。移動した後の点をＱ２’とする。
【０１３７】
この回転角θｄｂｖを両脚補償角という。始点がＱ１、終点がＱ１’のベクトルをベクトルＱ１Ｑ１’とする。同様に、始点がＱ２、終点がＱ２’のベクトルをベクトルＱ２Ｑ２’とする。図１６にＱ１’とＱ２’を示す。
【０１３８】
３）目標第１足平を、姿勢は変えずにベクトルＱ１Ｑ１’だけ平行移動（ほぼ上下移動）させる。同様に、目標第２足平を、姿勢は変えずにベクトルＱ２Ｑ２’だけ平行移動させる。移動後の目標各足平を図１６に太線で示す。
【０１３９】
４）次に、目標第１足平をＱ１’を中心に、前後方向軸（Ｘ軸）まわりに回転角θ１ｘ、左右方向軸（Ｙ軸）まわりに回転角θ１ｙだけ回転させる。同様に、目標第２足平を目標第２足平をＱ２’を中心に前後方向軸（Ｘ軸）まわりに回転角θ２ｘ、左右方向軸（Ｙ軸）まわりに回転角θ２ｙだけ回転させる。回転角θｎｘ，θｎｙをそれぞれ第ｎ足平Ｘ補償角、第ｎ足平Ｙ補償角という。回転後の目標各足平を図１７に太線で示す。
【０１４０】
以上の補償動作量が過大でなければ、接地圧力分布は変わっても、接地領域（足底面の圧力が正の領域）は変わらない。このような場合には、補償動作量に比例して各足平に装着されたばね機構３２や足底弾性体３４などが変形し、変形量に応じた実各足平床反力が発生する。この結果、補償動作量と補償動作によって発生する実床反力の変化量との間の関係は、以下に示す良好な特性を持つ。
【０１４１】
特性１）両脚補償角θｄｂｖだけを操作して目標各足平位置を移動させると、下がった足平の実足平床反力の力成分が増加し、上がった足平の実足平床反力の力成分が減少する。このとき、修正目標各足平床反力中心点まわりの実各足平床反力モーメントは、ほとんど変化しない。
【０１４２】
特性２）第ｎ足平Ｘ補償角だけを操作して目標第ｎ足平姿勢を回転させると、目標第ｎ足平床反力中心点に作用する実第ｎ足平床反力のモーメントのＸ成分だけが変化し、その他の床反力成分は少ししか変化しない。同様に、第ｎ足平Ｙ補償角だけを操作して目標第ｎ足平姿勢を回転させると、実第ｎ足平床反力のモーメントのＹ成分だけが変化し、その他の床反力成分は少ししか変化しない。
【０１４３】
特性３）両脚補償角θｄｂｖ、各足平Ｘ補償角および各足平Ｙ補償角を同時に操作すると、実各足平床反力の変化量は、それぞれを単独に操作したときの変化量の和になる。
【０１４４】
特性１および特性２は、これらの操作に独立性があることを示し、特性３はこれらの操作に線形性があることを示していると言える。
【０１４５】
図１８は複合コンプライアンス動作決定部の演算処理を示すブロック図であり、同図を参照してこの作業を説明する。
【０１４６】
概説すると、補償全床反力モーメント分配器において補償全床反力モーメントＭｄｍｄの分配を行う。次に、実各足平床反力と分配された補償全床反力モーメントなどから、両脚補償角決定部および第ｎ足平Ｘ（Ｙ）補償角決定部において前述の補償角θｄｂｖおよびθｎｘ（ｙ）を決定する。
【０１４７】
次に、決定された各種補償角に基づいて修正目標足平位置・姿勢算出部は、補償された足平位置・姿勢（これを修正目標足平位置・姿勢という）を幾何学演算によって求める。最後に、機構変形補償入り修正目標足平位置・姿勢算出部は、目標各足平床反力によって発生が予想されるばね機構３２や足底弾性体３４の変形量を求め、それらを打ち消すように修正目標足平位置・姿勢をさらに修正する。
【０１４８】
以下詳説すると、補償全床反力モーメント分配器は、補償全床反力モーメントＭｄｍｄを、両脚補償モーメントＭｄｍｄｄｂ、各足平補償モーメントＭｄｍｄ１ｘ，ｙ，Ｍｄｍｄ２ｘ，ｙに分配する。両脚補償モーメントＭｄｍｄｄｂは、両脚補償角（足平上下量）θｄｂｖを操作することによって目標全床反力中心点（目標ＺＭＰ）まわりに各足平床反力の力成分が作るモーメントの目標値である。
【０１４９】
両脚補償モーメントＭｄｍｄｄｂのＶ方向まわりの成分をＭｄｍｄｄｂｖと記述する。尚、ベクトルＶは複合コンプライアンス動作決定部の説明で定義したベクトルである。Ｖに直交し、鉛直方向にも直交するベクトルをＵとすると、両脚補償モーメントＭｄｍｄｄｂのＵ方向成分Ｍｄｍｄｄｂｕは０に設定される。両脚補償角θｄｂｖを操作しても、床反力のＵ方向モーメント成分を発生することはできないからである。
【０１５０】
この実施の形態では補償全床反力モーメントＭｄｍｄの鉛直方向成分が０なので、Ｍｄｍｄｄｂの鉛直方向成分Ｍｄｍｄｄｂｚも０に設定される。
【０１５１】
第１足平補償モーメントＭｄｍｄ１は、第１足平補償角θ１ｘ，θ１ｙを操作することによって目標第１足平床反力中心点まわりに発生させたいモーメントである。第１足平補償モーメントＭｄｍｄ１のＸ成分をＭｄｍｄ１ｘ、Ｙ成分をＭｄｍｄｌｙと記述する。第２足平補償モーメントＭｄｍｄ２は、第２足平補償角θ２ｘ，θ２ｙを操作することによって目標第２足平床反力中心点まわりに発生させたいモーメントである。第２足平補償モーメントＭｄｍｄ２のＸ成分をＭｄｍｄ２ｘ、Ｙ成分をＭｄｍｄ２ｙと記述する。
【０１５２】
分配は、例えば次のように行う。
Figure 0003629142
【０１５３】
ここで、Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔは分配用重み変数である。ＶｘはベクトルＶのＸ成分の値、ＶｙはベクトルＶのＹ成分の値である。この中で、Ｗｉｎｔは、両脚補償角を操作することによって発生した全床反力モーメントを各足平補償角を操作することによって打ち消すためのものである。
【０１５４】
式３と式４の演算処理を行う補償全床反力モーメント分配器のブロック図を図１９に示す。
【０１５５】
歩行時の分配用重み変数Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔの設定例を図２０に示す。図２０のパターンは、以下の注意点を考慮して決定することが望ましい。
【０１５６】
注意点１）両脚補償角と各足平補償角が不連続的に変化すると、関節に過大なトルクが発生する。そこで、両脚補償角と各足平補償角を連続的に変化させるために、分配用重み変数は連続的に変化させる。
【０１５７】
注意点２）両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントが、なるべく補償全床反力モーメントＭｄｍｄに近い値になるように、分配用重み変数を決定する。
【０１５８】
この際、直立時や歩行時など状況に応じて以下に示すように設定方針を変えた方が良い。直立時などのように、両脚補償モーメントのＶ方向成分Ｍｄｍｄｄｂｖ、各足平補償モーメントＭｄｍｄ１，Ｍｄｍｄ２を忠実に実各足平床反力に発生させることができる状況では以下のように設定する。
【０１５９】
この状況では目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平方向成分を、補償全床反力モーメントＭｄｍｄの水平方向成分に一致させるために、（即ち、前述の複合コンプライアンス動作決定部に対する要求１を満足するために、）式５と式６の両方をなるべく満足するように重みを設定すべきである。
【０１６０】
Figure 0003629142
【０１６１】
これに式３、式４を代入すると、式５は式７に、式６は式８に変換される。
Figure 0003629142
【０１６２】
ＭｄｍｄｘとＭｄｍｄｙが任意の値を取っても、式７と式８が恒等的に成立するためには、式９、式１０、および式１１を同時に満足すれば良い。
Figure 0003629142
即ち、以上の状況では式９、式１０および式１１を同時に満足するように、重みを決定すれば良い。
【０１６３】
歩行時ではＭｄｍｄｄｂｖを目標にして両脚補償角θｄｂｖを操作して足平の位置を修正しても、実全床反力モーメントの発生量がＭｄｍｄｄｂｖに較べて不足する場合がある。例えば図２１のように両脚支持期の初期にロボットが後傾して第１足平が未だ着地していない状況では、θｄｂｖによって第１足平の位置を下げても、実床反力は変化しない。
【０１６４】
同様に、Ｍｄｍｄ２を目標にして第２足平補償角θ２を操作して第２足平の角度を修正しても、実床反力モーメントの増加量がＭｄｍｄ２に較べて不足する場合がある。例えば、図２２のように両脚支持期の後半にロボットが後傾している状況では、θ２によって第２足平のかかとを下げても実床反力は変化しない。
【０１６５】
従って、式５、式６を満足するように各重みを設定しても、複合コンプライアンス制御によって発生する実全床反力の増加量が補償全床反力モーメントＭｄｍｄに届かない場合がある。このようなことが生じる可能性が高い状況では、式５、式６の左辺の値を右辺で割った値を１より大きくすべきである。
【０１６６】
歩行時の分配用重み変数設定例である図２０では、Ｗｉｎｔを０に設定することによって、図２１の状況のように、両脚補償角θｄｂｖ を操作しても実全床反力モーメントが発生できなくなっても、各足平補償角を操作して不足分を補うようにした。
【０１６７】
好都合なことに、図２１のように後傾すると第２足平のかかとが結果的に下がって床に接地しやすくなるので、第２足平補償角を操作することによって実全床反力モーメントを発生させることができるようになる。
【０１６８】
また、後傾していないときには両脚補償角θｄｂｖを操作することによる実全床反力モーメントが発生するが、第２足平のかかとが床に接地しないので、第２足平補償角を操作しても実全床反力モーメントは発生しない。
【０１６９】
つまり、両脚補償角θｄｂｖが有効に働くときには各足平補償角が有効に働かず、各足平補償角が有効に働くときには両脚補償角θｄｂｖが有効に働かないので、結果的に両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントの総量は、ほぼ補償全床反力モーメントＭｄｍｄに等しくなる。
【０１７０】
状況によっては、両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントの総量が補償全床反力モーメントＭｄｍｄよりも大きくなってしまう場合がある。
【０１７１】
しかし、この場合でも、Ｍｄｍｄがこの実施の形態のように姿勢安定化のためのフィードバック操作量であるならば、あまり問題にならない。何故ならば、Ｍｄｍｄの大きさが多少違っていても、一般的に制御系に言えることであるが、制御系のオープンループゲインが多少変化するだけで、クローズドループ特性はほとんど変わらないからである。
【０１７２】
注意点３）片脚支持期では、両脚補償角用の分配用重み変数であるＷｄｂｘ，Ｗｄｂｙの絶対値を小さくする。片脚支持期では両脚補償角を変化させても、接地していない足平が無駄に上下するだけで、実各足平床反力は変化しないからである。
【０１７３】
注意点４）足平の接地性を確保するために、目標足平床反力の力成分が小さいときには、その足平の足平補償角のための分配用重み変数の絶対値を小さくする。特に、足平が床から遠く離れているときには、その足平の足平補償角を動かしても、その足平の実足平床反力は変化しないので、不要な動きをさせないためにも、その足平の足平補償角のための分配用重み変数の絶対値を小さくすべきである。
【０１７４】
注意点５）両脚補償角を操作することによって制御できる実全床反力モーメントの方向と、各足平補償角を操作することによって制御できる実全床反力モーメントの方向は通常異なる。
【０１７５】
例えば、両脚補償角θｄｂｖを操作することによって発生する実全床反力モーメントの向きは必ずＶ方向であり、Ｖ方向に直交する成分を発生させることはできない。一方、各足平補償角を操作することによって発生できる実全床反力モーメントの向きは、足平の接地状況によって制約を受ける。
【０１７６】
例えば、つまさきのエッジだけまたはかかとのエッジだけが接地している場合には、エッジ線方向にモーメントを発生することはできない。両脚支持期では、この特性を考慮して、なるべく無駄なく両脚補償角および各足平補償角を操作する。
【０１７７】
例えば、両脚補償角を操作するための分配重みＷｄｂｘ，Ｗｄｂｙは次のように決定する。
【０１７８】
Ｘ成分がＷｄｂｘ、Ｙ成分がＷｄｂｙ、Ｚ成分が０のベクトルをＷｄｂとすると、式３はベクトルＷｄｂとＭｄｍｄの内積になっている。従って、ＭｄｍｄをベクトルＷｄｂ方向成分とその直交成分に分解し、ベクトルＷｄｂ方向成分だけを抽出して、ベクトルＷｄｂの大きさを乗じたものが、式３によって求められるＭｄｍｄｄｂｖであると言える。
【０１７９】
この場合のＭｄｍｄｄｂｖを図２３に示す。これは、両脚補償角を操作することによって実全床反力モーメントのＷｄｂ方向成分を制御するフィードバック制御系を構成することを意味する。もし、Ｗｄｂ方向がベクトルＶと直交していたら、両脚補償角をいくら操作しても実全床反力モーメントのＷｄｂ方向成分は発生しないから、このフィードバック制御系はただ無駄に両脚補償角を操作するだけになる。
【０１８０】
従って、無駄な動きを減らした場合には、Ｗｄｂ方向をベクトルＶ方向に一致させるか、またはなるべく近づけるべきである。また、補償全床反力モーメントＭｄｍｄのＷｄｂ方向成分を、各足平補償角に頼らずに両脚補償角を操作するだけで発生させたいならば、ＷｄｂとＶの内積が１になるように設定する。一部を各足平補償角に頼らせたいならば、ＷｄｂとＶの内積が１より小さくなるように設定する。
【０１８１】
ところで、足平の横幅が狭い場合には、各足平補償角を操作することによって発生し得る実各足平床反力モーメントのＸ成分は小さくなる。この場合には、Ｗｄｂｘを大きめに設定する。Ｗｄｂ方向とベクトルＶ方向は一致しなくなり、両脚補償角の変動が増加するが、安定性が増す。
【０１８２】
両脚補償角決定部について更に詳説すると、図２４は両脚補償角決定部の演算処理のブロック図であり、両脚補償角θｄｂｖは図示の如く演算される。
【０１８３】
図２４を参照して説明すると、目標第１足平床反力中心点Ｑ１に作用するＦ１ａｃｔと目標第２足平床反力中心点Ｑ２に作用するＦ２ａｃｔが、目標全床反力中心点Ｐのまわりに発生させるモーメントＭｆ１ｆ２ａｃｔを、次式により求める。
【０１８４】
Figure 0003629142
ここで、ＰＱ１は始点がＰ、終点がＱ１のベクトル、ＰＱ２は始点がＰ、終点がＱ２のベクトルである。
【０１８５】
また、式１２の代わりに、次式を用いても実際上はほとんど問題がない。
Ｍｆ１ｆ２ａｃｔ ＝ ＰＱ１＊Ｆ１ａｃｔ ＋ ＰＱ２＊Ｆ２ａｃｔ＋ Ｍ１ａｃｔ＋ Ｍ２ａｃｔ ・・・式１２ａ
式１２ａは、目標全床反力中心点まわりに作用する実全床反力モーメントＭａｃｔを算出する式になっている。尚、式１２は、目標全床反力中心点まわりに作用する実全床反力モーメントから、目標各足平床反力中心点まわりに作用する実各足平床反力モーメントを減じたものになっている。請求項４項の記載は、これに基づく。
【０１８６】
次に、Ｍｆ１ｆ２ａｃｔのベクトルＶ方向成分Ｍｆ１ｆ２ａｃｔｖを抽出する。これは、ベクトルの内積演算を用いた次式によって得られる。尚、ベクトルＶは前述の動作説明において図１５に示したＶである。
Figure 0003629142
【０１８７】
次に、Ｍｆ１ｆ２ａｃｔｖをローパスフィルタに通してＭｆ１ｆ２ａｃｔｖｆｉｌｔを得る。
【０１８８】
次に、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを補償用フィルタに通し、それを、Ｍｆ１ｆ２ａｃｔｖｆｉｌｔから減じ、偏差モーメントＶ方向成分Ｍｄｉｆｆｖを得る。
【０１８９】
尚、補償用フィルタは、Ｍｄｍｄｄｂｖから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。
【０１９０】
次に、足平ばね機構などの変形による両脚補償モーメントＶ方向成分への影響を打ち消すための両脚機構変形補償角θｆｆｄｂｖを求める。これは、いわゆるフィードフォワード補償である。
【０１９１】
具体的には、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖと変形量との関係を表す機構コンプライアンスモデルを用い、目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を結ぶ線分の変形角度を求め、それの極性を反転したものを両脚機構変形補償角θｆｆｄｂｖとすれば良い。
【０１９２】
両脚機構変形補償角θｆｆｄｂｖは、近似的には次式により求めれば良い。
Figure 0003629142
ここでαは所定の定数である。
【０１９３】
最後に次式によって両脚補償角θｄｂｖを得る。ここでＫｄｂは制御ゲインであり、通常、これは正の値に設定する。
Figure 0003629142
【０１９４】
第ｎ足平補償角決定部について説明すると、図２５はその中の第１足平Ｘ補償角決定部の演算処理を示すブロック図であり、第１足平Ｘ補償角θ１ｘは図示の如く演算する。説明は省略するが、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙも同様に求める。ここでは第１足平Ｘ補償角θ１ｘを求めるアルゴリズムだけを説明する。
【０１９５】
第１足平床反力モーメントＸ成分Ｍ１ａｃｔｘをローパスフィルタに通してＭ１ａｃｔｆｉｌｔｘを得る。第１足平補償モーメントＸ成分Ｍｄｍｄ１ｘを補償用フィルタに通し、それを、Ｍ１ａｃｔｆｉｌｔｘから減じ、偏差モーメントＭｄｉｆｆ１ｘを得る。両脚補償角決定と同様、補償用フィルタは、Ｍｄｍｄ１ｘから実全床反力までの伝達関数の周波数応答特性を改善するものである。
【０１９６】
次に、両脚補償角決定と同様、足平ばね機構などの変形による第１足平補償モーメントＸ成分への影響を打ち消すための第１足平Ｘ機構変形補償角θｆｆ１ｘを求める。これは、いわゆるフィードフォワード補償である。
【０１９７】
具体的には、第１足平補償モーメントＸ方向成分Ｍｄｍｄ１ｘと変形量との関係を表す機構コンプライアンスモデルを用い、第１足平の変形角度を求め、それの極性を反転したものを第１足平Ｘ機構変形補償角θｆｆ１ｘとすれば良い。
【０１９８】
第１足平Ｘ機構変形補償角θｆｆ１ｘは、近似的には次式により求めれば良い。
Figure 0003629142
ここでα１ｘは所定の定数である。
【０１９９】
最後に次式によって第１足平Ｘ補償角θ１ｘを得る。ここでＫ１ｘは制御ゲインであり、通常、これも正の値に設定する。
Figure 0003629142
尚、図示のブロック線図は、演算処理順序を変えるなどの等価変形をしても良い。
【０２００】
図１８に戻って説明を続けると、修正目標足平位置・姿勢算出部は、両脚補償角θｄｂｖ、第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙ、および同位相補償変位（後述）に基づき、前述の複合コンプライアンス動作を拡張した足平位置・姿勢修正手法に従って目標足平位置・姿勢を修正し、修正目標足平位置・姿勢を得る。
【０２０１】
機構変形量算出部は、目標各足平床反力によって発生が予想されるばね機構３２や足底弾性体３４の変形量を求める。
【０２０２】
機構変形補償入り修正目標足平位置・姿勢算出部は、算出された機構変形量を打ち消すように、修正目標足平位置・姿勢をさらに修正し、機構変形補償入り修正目標足平位置・姿勢を得る。
【０２０３】
例えば、図２６に示すような機構変形量が予想されるときには、機構変形補償入り修正目標足平位置・姿勢は、図２７に実線で示す位置・姿勢に修正される。即ち、図２７に示す機構変形補償後の足平が目標足平床反力を受けて変形したときの位置・姿勢が、図２６に示す床反力を受けないときの機構変形補償前の足平位置・姿勢に一致するように、機構変形補償入り修正目標足平位置・姿勢を算出する。
【０２０４】
機構変形補償は、ばね機構３２や足底弾性体３４の変形によって生じる実足平位置・姿勢のずれをフィードフォワード的に打ち消す制御であり、この制御がない場合に比較し、より一層、目標歩容に近い歩行を実現することができる。
【０２０５】
上記を前提として図１０フロー・チャートの説明に戻ると、前記の如く、Ｓ３４において上記した補償角を決定する。
【０２０６】
図２８はその作業を示すサブルーチン・フロー・チャートである。
【０２０７】
同図を参照して説明すると、先ずＳ１００において前記したベクトルＶを求め、Ｓ１０２に進んで分配用重み変数を図２０に示すように設定し、現在時刻ｔでのこれらの値を求める。続いてＳ１０４に進み、式３および式４によって補償全床反力モーメントＭｄｍｄを両脚補償モーメントＭｄｍｄｄｂｖと各足平補償モーメントＭｄｍｄｎｘ（ｙ）に分配し、Ｓ１０６に進んで既述の如く両脚補償角θｄｂｖを求め、Ｓ１０８に進んで各足平補償角θｎｘ（ｙ）を求める。
【０２０８】
図１０フロー・チャートの説明に戻ると、続いてＳ３５に進んで同位補償変位を決定するが、これについては後述する。
【０２０９】
続いてＳ３６に進み、目標各足平床反力に基づいて機構変形補償量を算出し、Ｓ３８に進んで目標足平位置・姿勢を補償角θｄｂｖ，θｎｘ（ｙ）、および同位相補償変位（後述）に応じて修正し、更にこれを機構変形補償量に応じて修正し、機構変形補償入り修正目標足平位置・姿勢を得る。
【０２１０】
次いでＳ４０に進み、上***置・姿勢と機構変形補償入り修正足平位置・姿勢から関節変位指令（値）を算出し、Ｓ４２に進んで実関節変位を算出された関節変位指令（値）にサーボ制御し、Ｓ４４に進んで時刻をΔｔ更新し、Ｓ１４に戻って上記の処理を繰り返す。
【０２１１】
ここで、図１０フロー・チャートのＳ３１およびＳ３５の処理について説明する。
【０２１２】
これらは、上体加速度を検出し、それが過大または過少にならないように、補償全床反力の力成分（目標値）Ｆｄｍｄを後述する制御則によって決定し、全足平を同時に同じ向きに平行移動させて実全床反力の力成分の高周波成分をＦｄｍｄの高周波成分に近づける制御である。
【０２１３】
この制御を行うと、例えば、歩行中に予期しない段差によってロボット１が落下して足平２２ＲあるいはＬが床面に強く着地したとき、両足平２２Ｒ（Ｌ）を僅かに重力方向に引き上げることで、着地衝撃を吸収することができる。
【０２１４】
また、ロボットは、歩行中、足平のばね機構３２や足底弾性体３４の上下伸縮の弾性とロボット１の質量によって決定される固有振動を持ち、上体２４が上下に振動する。この振動は変位量としては僅かであるが、実足平床反力のＺ方向力成分Ｆｚは大きく変動する。
【０２１５】
この振動の振幅が過大になると、足平２２Ｒ（Ｌ）と床面との接地性が低下し、場合によってロボット１はスピンする。この制御はかかる振動を抑制し、スピンを防止して姿勢安定化を向上させる。その意図から、この実施の形態においては、先に提案した装置に、図４の下部に示す如く、上体Ｚ方向加速度制御演算部を追加した。
【０２１６】
上体Ｚ方向加速度制御演算部は、傾斜センサ６０によって検出された実上体加速度Ｇｂｏｄｙ（より具体的には例えば、検出された実上体上下加速度Ｇｂｏｄｙｚ）が、目標歩容の上体加速度（即ち、目標上体加速度）Ｇｂｏｄｙｒｅｆから大きくずれないように補償全床反力の力成分（目標値）Ｆｄｍｄを求める如くした。これが、Ｓ３１の処理に相当する。
【０２１７】
以下説明すると、Ｆｄｍｄは、例えば、次のように算出する。即ち、ＧｂｏｄｙｒｅｆのＸ成分をＧｂｏｄｙｒｅｆｘ，Ｘ成分をＧｂｏｄｙｒｅｆｙ，Ｚ成分をＧｂｏｄｙｒｅｆｚとすると、次式に従って目標値と検出値の偏差Ｆｄｍｄｔｍｐを算出する。
【０２１８】
Figure 0003629142
【０２１９】
ここで、Ｋｇｘ，Ｋｇｙ，Ｋｇｚは、制御ゲイン（定数）である。Ｆｄｍｄｔｍｐｘ，Ｆｄｍｄｔｍｐｙ，Ｆｄｍｄｔｍｐｚはそれぞれ、前記偏差ＦｄｍｄｔｍｐのＸ，Ｙ，Ｚ成分であり、それらを合算して偏差Ｆｄｍｄｔｍｐとする。続いて、算出したＦｄｍｄｔｍｐを制御特性改善用に適宜設計したフィルタに通し、その出力をＦｄｍｄとする。
【０２２０】
ここで、目標全床反力中心点を始点、ロボット重心位置を終点とするベクトルを（Ｊｘ，Ｊｙ，Ｊｚ）とすると、上記したＫｇｘ，Ｋｇｙ，Ｋｇｚの比を、Ｊｘ，Ｊｙ，Ｊｚに一致させると良い。そうすると、Ｆｄｍｄもベクトル（Ｊｘ，Ｊｙ，Ｊｚ）の向きになる。
【０２２１】
従って、Ｆｄｍｄを床反力として目標全床反力中心点に追加的に発生させても、Ｆｄｍｄはロボット重心位置に向かって作用するので、ロボット１を回転させるモーメントが発生せず、ロボット１の姿勢傾きに影響を及ぼすことがない。尚、Ｆｄｍｄによって決定される全足平の平行移動量が僅かである場合には、ＫｇｘとＫｇｙを零にしても良い。そうしても、同様に、ロボット１の姿勢傾きに影響を及ぼすことがないからである。
【０２２２】
続いてＳ３５の処理について説明する。
【０２２３】
図２９を参照して概説すると、実全床反力の力成分を操作するため、図中の同位相補償変位を無視してＳ３８までの処理で得られた修正目標第１足平位置姿勢（図２９に細線で示す）を、姿勢を変えずに、ある移動量ベクトルＬ１だけ平行移動させる。
【０２２４】
同時に、修正目標第２足平位置姿勢も、姿勢を変えずに、ある移動量ベクトルＬ２だけ平行移動させる。移動後の修正目標第１、第２足平位置姿勢を図２９に太線で示す。
【０２２５】
これが、実全床反力の力成分に対するコンプライアンス制御であり、この明細書では「同位相平行移動動作」といい、移動ベクトルＬｎを第ｎ足平同位相補償変位という（ｎ＝１，２）。
【０２２６】
即ち、図１８の下部に示す如く、同位相補償変位算出部を設け、補償全床反力の力成分（目標値）と実全床反力の力成分から各足平同位相補償変位を算出し、修正目標足平位置姿勢算出部では、各足平同位相補償変位も含めて修正目標足平位置姿勢を算出するようにした。
【０２２７】
図３０はその作業をさらに詳細に示すブロック図であり、図３１は図３０の作業を説明するフロー・チャートである。
【０２２８】
以下図３１を参照して説明すると、Ｓ２００において実全床反力の力成分Ｆｔｏｔａｌａｃｔを求める。即ち、実第１足平床反力の力成分Ｆ１ａｃｔと実第２足平床反力の力成分Ｆ２ａｃｔから次の式（式１９）によって実全床反力の力成分Ｆｔｏｔａｌａｃｔを算出する。
Ｆｔｏｔａｌａｃｔ＝Ｆ１ａｃｔ＋Ｆ２ａｃｔ ．．．式１９
【０２２９】
続いてＳ２０２に進み、Ｆｔｏｔａｌａｃｔから目標全床反力（即ち、目標歩容の力成分Ｆｒｅｆ）を減算して偏差Ｆｔｏｔａｌｅｒｒを求め、Ｓ２０４に進んで適宜なフィルタを通してＦｔｏｔａｌｅｒｒｆｉｌｔを得る。
【０２３０】
次いでＳ２０６に進み、補償全床反力の力成分（目標値）Ｆｄｍｄを補償用フィルタに通し、フィルタ通過値からＦｔｏｔａｌｅｒｒｆｉｌｔを減算して差Ｆｔｏｔａｌｄｉｆｆを得る。この補償用フィルタは、Ｆｄｍｄから実全床反力の力成分までの周波数応答性を改善するものである。
【０２３１】
次いでＳ２０８に進み、次式（式２０）によって同位相補償変位ベクトルＬを算出する。
Ｌ＝Ｋ１＊Ｆｔｏｔａｌｄｉｆｆ ．．．．式２０
ここで、Ｋ１は制御ゲイン（定数。スカラ量）である。
【０２３２】
次いで、算出されたベクトルＬを以下の式によって各足平に配分する。
Figure 0003629142
ここで、Ｗ１１，Ｗ１２はアッテネータ（重み）であり、例えば図３２に示すように設定する。
【０２３３】
このＷ１１，Ｗ１２は常に１でも良いが、足平２２Ｒ（Ｌ）が離床した後は上下に無駄に動くことを避けるために、空中にあるときは（図示の例では片脚支持期に第２足平側）０にすると良い。
【０２３４】
かく得られた同位相補償変位Ｌｎも含め、図１０フロー・チャートのＳ３８において機構変形補償入り修正目標足平位置姿勢が算出される。
【０２３５】
具体的には、両脚補償角θｄｂｖ、第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙ、第１足平同位相補償変位Ｌ１、および第２足平同位相補償変位Ｌ２に基づき、前記した同位相平行移動動作を含む複合コンプライアンス動作に従って修正目標足平位置姿勢が算出される。
【０２３６】
この実施の形態では上記の如く、少なくとも基体（上体２４）と、前記基体に第１の関節（１０，１２，１４Ｒ（Ｌ））を介して連結されると共に、その先端に第２の関節（１８，２０Ｒ（Ｌ））を介して連結される足部（足平２２Ｒ（Ｌ））を備えた複数本（２本）の脚部（脚部リンク２）からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上***置・姿勢、目標足平位置・姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（＝目標ＺＭＰ））を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（歩容生成器、Ｓ１０からＳ２２）、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、Ｓ２４，Ｓ２６）、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（６軸力センサ４４、実各足平床反力検出器、Ｓ３２）、前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモーメント（実第ｎ足平床反力モーメントＭａｃｔｘ，ｙ，ｚ）を算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる回転量（両脚補償角θｄｂｖ，ｚ、第ｎ足平補償角θｎｘ，ｙ，ｚ）を決定する足部回転量決定手段（複合コンプライアンス動作決定部、Ｓ３２からＳ３４、両脚補償角決定部、第ｎ足平補償角決定部、Ｓ１００からＳ１０８）、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が回転するように前記目標位置および／または姿勢を修正する第１の足部位置・姿勢修正手段（複合コンプライアンス動作決定部、Ｓ３８，Ｓ４０、修正目標足平位置・姿勢算出部）、前記検出された実床反力から実全床反力の力成分Ｆｔｏｔａｌａｃｔを求めて前記生成された歩容の全床反力の力成分Ｆｒｅｆとの偏差Ｆｔｏｔａｌｅｒｒを算出し、前記算出した偏差に基づいて前記足部の補償変位Ｌｎを算出する補償変位算出手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３１，Ｓ３５，Ｓ２００からＳ２０８）、前記算出された補償変位に基づいて目標足部位置・姿勢を修正する第２の足部位置・姿勢修正手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３８）および前記第１および前記第１および第２の足部位置・姿勢手段によって修正された足部の位置・姿勢に基づいて前記ロボットの第１および第２の関節（１０，１２，１４，１８，２０Ｒ（Ｌ））を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、Ｓ４０，Ｓ４２）を備えるように構成した。
【０２３７】
また、少なくとも基体（上体２４）と、前記基体に第１の関節（１０，１２，１４Ｒ（Ｌ））を介して連結されると共に、その先端に第２の関節（１８，２０Ｒ（Ｌ））を介して連結される足部（足平２２Ｒ（Ｌ））を備えた複数本（２本）の脚部（脚部リンク２）からなる脚式移動ロボット１の制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上***置・姿勢、目標足平位置・姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（＝目標ＺＭＰ））を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（歩容生成器、Ｓ１０からＳ２２）、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、Ｓ２４）、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（６軸力センサ４４、実各足平床反力検出器、Ｓ３２）、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量（両脚補償角θｄｂｖ，ｚ、第ｎ足平補償角θｎｘ，ｙ，ｚ）を決定する足部回転量決定手段（複合コンプライアンス動作決定部、Ｓ３２，Ｓ３４、両脚補償角決定部、第ｎ足平補償角決定部、Ｓ１００からＳ１０８）、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および／または姿勢を修正する第１の足部位置・姿勢修正手段（複合コンプライアンス動作決定部、Ｓ３８，Ｓ４０、修正目標足平位置・姿勢算出部）、前記検出された実床反力から実全床反力の力成分Ｆｔｏｔａｌａｃｔを求めて前記生成された歩容の全床反力の力成分Ｆｒｅｆとの偏差Ｆｔｏｔａｌｅｒｒを算出し、前記算出した偏差に基づいて前記足部の補償変位Ｌｎを算出する補償変位算出手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３１，Ｓ３５，Ｓ２００からＳ２０８）、前記算出された補償変位に基づいて目標足部位置・姿勢を修正する第２の足部位置・姿勢修正手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３８）および前記第１および前記第１および第２の足部位置・姿勢手段によって修正された足部の位置・姿勢に基づいて前記ロボットの第１および第２の関節（１０，１２，１４，１８，２０Ｒ（Ｌ））を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、Ｓ４２）を備えるように構成した。
【０２３８】
また、前記第１の足部位置・姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および／または姿勢を修正する如く構成した。
【０２３９】
さらに、前記ロボットに実際に作用する全床反力モーメント（より正確にはモーメント成分ＰＱ１ ＊Ｆ１ａｃｔ ＋ＰＱ２ ＊Ｆ２ａｃｔ ＋Ｍ１ａｃｔ ＋Ｍ２ａｃｔ）、または前記ロボットに実際に作用する全床反力のモーメント（ＰＱ１ ＊Ｆ１ａｃｔ ＋ＰＱ２ ＊Ｆ２ａｃｔ ＋Ｍ１ａｃｔ ＋Ｍ２ａｃｔ ）から前記足部に作用する床反力モーメント（Ｍ１ａｃｔ ＋Ｍ２ａｃｔ）を減算して得たモーメント（Ｍｆ１ｆ２ａｃｔ＝ＰＱ１ ＊Ｆ１ａｃｔ ＋ＰＱ２ ＊Ｆ２ａｃｔ ）のいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる移動量（θｄｂｖ，ｚ）を決定する足部移動量決定手段（複合コンプライアンス動作決定部、Ｓ３４、両脚補償角決定部、Ｓ１００からＳ１０８）を備え、前記第１の足部位置・姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置および／または姿勢を修正するように構成した。
【０２４０】
また、前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モーメント（補償全床反力Ｍｄｍｄ）を求め、前記足部回転量決定手段および／または前記足部移動量決定手段は、少なくとも前記検出された実床反力（実各足平床反力）と前記求めた姿勢安定化補償全床反力モーメントに基づいて前記足部の回転量および／または移動量を決定する（Ｓ３４，Ｓ１００からＳ１０８）如く構成した。
【０２４１】
また、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差（θｅｒｒｘ，ｙ）に基づいて求める（Ｓ２８，Ｓ３０）如く構成した。
【０２４２】
また、前記補償変位算出手段は、前記ロボットの基体加速度（実上体上下加速度）Ｇｂｏｄｙを検出する基体加速度検出手段（Ｓ３１）、および前記検出された基体加速度に基づき、前記ロボットの姿勢を安定化するための補償床反力の力成分（の目標値Ｆｄｍｄ）を算出する補償全床反力力成分算出手段（Ｓ３１）を備え、前記算出された補償全床反の力成分から前記偏差を減算して得た差（ＦｔｏｔａｌｄｉｆｆあるいはＬ）に基づいて前記足部の補償変位Ｌｎを算出する（Ｓ２００からＳ２０８）如く構成した。
【０２４３】
また、前記補償変位算出手段は、前記差（ＦｔｏｔａｌｄｉｆｆあるいはＬ）に所定の重み（アッテネータＷｌ１，Ｗｌ２）を乗じて前記足部の補償変位を足部のそれぞれに分配する如く構成した。
【０２４４】
また、前記補償変位算出手段は、前記足部が離床しているとき、前記重みを零にする如く構成した。
【０２４５】
また、前記補償変位算出手段は、前記重みを前記実全床反力の力成分の周波数に応じて可変にする如く構成した。
【０２４６】
また、前記補償変位算出手段は、前記足部の補償変位を重力方向について算出する如く構成した。
【０２４７】
また、前記補償変位算出手段は、前記足部の補償変位を重心と目標全床反力中心点を結ぶ線分の方向について算出する如く構成した。
【０２４８】
また、前記補償全床反力の力成分を零またはその近傍に設定する如く構成した。
【０２４９】
上記の如く構成したので、これによって、概括すれば、実全床反力の制御と実各足平床反力の制御が殆ど干渉しないようになり、それらを容易に制御することができる。
【０２５０】
即ち、この実施の形態に係る装置は、先に提案した技術に対して以下の点を改良した。即ち、特開平５−３０５５８４号公報で提案した足首コンプライアンス制御では、足首または足底の基準点などの足平に固定された点における実床反力モーメントを検出し、それに基づいて前記固定された点を中心に足平を回転させていたが、この実施の形態に係る装置では、移動する目標足平床反力中心点における実各足平床反力モーメントを算出し、それに基づいて目標足平床反力中心点を中心に足平を回転させるように変更し、その点まわりのモーメントを望ましい値に制御するようにした。
【０２５１】
この結果、実全床反力と実各足平床反力がほとんど干渉することなく、容易に制御することが可能となった。より干渉を少なくするために、各瞬間における想定していた足底接地領域内にもっと適切な点を選定しても良い。
【０２５２】
従って、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。
【０２５３】
更には、ロボットに作用する床反力、より具体的には目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントと目標各足平中心点まわりの実各足平床反力モーメントを容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に比較して、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。
【０２５４】
また、ロボットが受ける実床反力の力成分に対してコンプライアンス制御を行うようにしたので、予期しない段差などに遭遇して過大な着地衝撃を生じても、その着地衝撃を吸収することができると共に、接地性を向上させることができてスピンの発生などを防止することができる。
【０２５５】
また、脚式移動ロボットの姿勢安定化制御を容易に実現できると共に、脚式移動ロボットが受ける着地衝撃を低減することができ、脚式移動ロボットの接地性を高め、歩行時のスリップやスピンを防止することができる。更に、脚式移動ロボットのアクチュエータの負荷を低減することができる。
【０２５６】
ここで、同位相補償変位Ｌの各足平への配分について補足すると、式２１においては、例えば重みＷｌ１，Ｗｌ２を１．０に固定、あるいは遊脚の場合０にするなど、換言すれば、実全床反力の力成分の周波数に対して固定した。
【０２５７】
そのように設定すると、実全床反力の力成分の低周波成分を任意に制御することはできない。何故ならば、例えば、直立時に両足平を重力方向に同位相平行移動させても（即ち、両脚を縮めても）、その瞬間には実全床反力の力成分が減少するが、その直後に上体２４は下に移動した時点で再び元の実全床反力に戻ってしまうからである。
【０２５８】
それを解消すべく、式２０に積分項を加えると、実全床反力の力の低周波成分まで制御することができる。しかし、高速歩行時には、上体高さが目標歩容から大きく変化すると、動バランスを崩すので、積分項を加えることは好ましくない。
【０２５９】
そこで、同位相補償変位Ｌの配分量を両足平で相違させても良く、あるいは実全床反力の力成分の周波数に対して可変にしても良い。即ち、周波数成分を持った重みにしても良い。
【０２６０】
それについて以下説明すると、図３３に示す如く、重みＷｌ１，Ｗｌ２を実全床反力の力成分の周波数に対して可変に設定する。具体的には、所定値、例えば５Ｈｚよりも十分に低い周波数領域にあっては、重みＷｌ１，Ｗｌ２の比を１付近に設定すると共に、それ以上の周波数領域では比が変わるように設定する。
【０２６１】
例えば、５Ｈｚよりも十分に低い周波数領域にあっては、Ｌ１：Ｌ２＝１：１となるように決定する。即ち、実全床反力の力の低周波成分に対しては、上体２４が足平２２Ｒ（Ｌ）の伸縮につられて重力方向に大きく動くので、いずれの足平も浮かないように、両足平の値を同一にする。
【０２６２】
他方、５Ｈｚ以上の高周波数領域にあっては、図２９において、目標全床反力中心点Ｐまわりにモーメントが発生しないように、その目標全床反力中心点Ｐと修正目標第ｎ足平床反力中心点Ｑｎ’との距離に反比例するように、設定する。より具体的には、図２９において、Ｌ１：Ｌ２＝ＰＱ２’：ＰＱ１’となるように決定する。
【０２６３】
尚、前述の式１８は、以下のように一般化しても良い。
Figure 0003629142
ただし、Ｋｇｘｘ，Ｋｇｘｙ，Ｋｇｘｚ，Ｋｇｙｘ，Ｋｇｙｙ，Ｋｇｙｚ，Ｋｇｚｘ，Ｋｇｚｙ，Ｋｇｚｚは、制御ゲインである。
【０２６４】
尚、上記した実施の形態において、上体加速度の検出および補償床反力の力成分（目標値）Ｆｄｍｄを決定する制御則を省略し、Ｆｄｍｄを零またはその近傍に固定するだけでも、実全床反力の力成分に対するコンプライアンス制御として、かなり有効である。尚、請求項１２項はこの記載に基づく。
【０２６５】
また、上記した実施の形態において、ばね機構３２（および足底弾性体３４）自身はこの発明の本質部分ではない。この発明の本質はフィードバック制御部分にあり、機構変形補償は付随的なものである。
【０２６６】
また、上記した実施の形態において、ブロック線図は演算処理順序を変えるなどの等価変形をしても良い。
【０２６７】
また、上記した実施の形態を、先に提案した出願の第１の実施の形態に関して説明したが、第２あるいはそれ以降の実施の形態に関して追加しても良い。
【０２６８】
また、この発明を２足歩行ロボットに関して説明してきたが、２足歩行ロボットに限らず、多脚ロボットにも応用することができる。
【０２６９】
【発明の効果】
請求項１項にあっては、脚式移動ロボットに作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に近い制御を行っても、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。
【０２７０】
また、脚式移動ロボットの姿勢安定化制御を容易に実現できると共に、脚式移動ロボットが受ける着地衝撃を低減することができ、脚式移動ロボットの接地性を高め、歩行時のスリップやスピンを防止することができる。
【０２７１】
さらには、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。また、脚式移動ロボットのアクチュエータの負荷を低減することができる。
【０２７２】
請求項２項にあっては、請求項１項と同様の作用効果を得ることができる。
【０２７３】
請求項３項にあっては、請求項１項と同様の作用効果を得ることができると共に、床反力をより適切に制御することができる。
【０２７４】
請求項４項にあっては、請求項１項と同様の作用効果を得ることができると共に、特に姿勢制御に重要な全床反力を一層適切に制御することができる。
【０２７５】
請求項５項にあっては、請求項１項と同様の作用効果を得ることができると共に、姿勢安定化能力を向上させることができる。
【０２７６】
請求項６項にあっては、前記したと同様の作用効果を得ることができると共に、姿勢安定化能力を一層向上させることができる。
【０２７７】
請求項７項にあっては、前記したと同様の作用効果を得ることができると共に、着地衝撃を一層効果的に吸収することができ、また接地性を一層向上させることができる。
【０２７８】
請求項８項にあっては、前記したと同様の作用効果を得ることができる。
【０２７９】
請求項９項にあっては、前記したと同様の作用効果を得ることができると共に、制御を必要限度に減少させることでアクチュエータの負荷を一層低減することができる。
【０２８０】
請求項１０項にあっては、前記したと同様の作用効果を得ることができると共に、一層安定した姿勢を得ることができる。
【０２８１】
請求項１１項にあっては、前記したと同様の作用効果を得ることができると共に、演算量を低減することができる。
【０２８２】
請求項１２項にあっては、前記したと同様の作用効果を得ることができると共に、演算量を低減することができる。
【０２８３】
請求項１３項にあっては、前記したと同様の作用効果を得ることができると共に、かなりの程度まで着地衝撃の吸収と接地性の向上を実現することができ、さらに構成を簡易にすることができる。
【図面の簡単な説明】
【図１】この発明に係る脚式移動ロボットの制御装置を全体的に示す説明図である。
【図２】図１に示す２足歩行ロボットの足部の構造を示す説明側面図である。
【図３】図１に示す２足歩行ロボットの制御ユニットの詳細を示すブロック図である。
【図４】この発明に係る脚式移動ロボットの制御装置の構成および動作を機能的に示すブロック図である。
【図５】図１に示す脚式移動ロボットが平地を歩行するときの運動パターンの一例を示す説明図である。
【図６】図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡の床面上軌跡を示す説明図である。
【図７】図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡のタイム・チャートである。
【図８】図５の運動パターンに対応する所定の条件を満たすように設定した目標第１足平床反力中心点軌跡のタイム・チャートである。
【図９】図５の運動パターンに対応する所定の条件を満たすように設定した目標第２足平床反力中心点軌跡のタイム・チャートである。
【図１０】図４と同様に、この発明に係る脚式移動ロボットの制御装置の動作を示すフロー・チャートである。
【図１１】図１０フロー・チャートの内の両脚補償角などの演算処理を行う、図４に示す複合コンプライアンス動作決定部の動作を説明するための、両脚支持期に第１足平と第２足平に実各足平床反力が作用している状況を示す説明図である。
【図１２】図１１に示す状況における目標全床反力の設定を示す説明図である。
【図１３】図１１に示す状況における目標各足平床反力の分配を示す説明図である。
【図１４】図１１に示す状況における補償全床反力モーメントを示す説明図である。
【図１５】図１１に示す状況における、各足平床反力中心点を含み、水平面に垂直な平面の法線ベクトルＶを示す説明図である。
【図１６】図１１に示す状況における、目標各足平床反力中心点を目標全床反力中心点（目標ＺＭＰ）まわりに、所定角度θｄｂｖだけ回転させたときの状態を示す説明図である。
【図１７】図１６に示す状況における、各足平を前後方向軸および左右方向軸まわりに所定角度θｎｘ，θｎｙだけ回転させたときの状態を示す説明図である。
【図１８】図４の複合コンプライアンス動作決定部の演算処理を示すブロック図である。
【図１９】図１８に示す補償全床反力モーメント分配器の演算処理を示すブロック図である。
【図２０】図１８に示す補償全床反力モーメント分配器の、両脚補償角などを操作するための分配重み変数の設定例を示すタイム・チャートである。
【図２１】図２０の補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。
【図２２】図２１と同様に、補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。
【図２３】両脚補償角を操作するための分配重みを所定の条件で決定したときの両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを示す説明図である。
【図２４】図１８に示す両脚補償角決定部の演算処理を示すブロック図である。
【図２５】図１８に示す各足平の補償角決定部の演算処理を示すブロック図である。
【図２６】図１８に示す機構変形補償入り修正目標足平位置・姿勢算出部の演算処理を説明するための説明図である。
【図２７】図２６と同様に、図１８に示す機構変形補償入り修正目標足平位置・姿勢算出部の演算処理を説明するための説明図である。
【図２８】図１０フロー・チャートの内の両脚補償角などの決定作業を示すサブルーチン・フロー・チャートである。
【図２９】図４の上体加速度制御演算部および図１８の同位相補償変位算出部の動作で、図１０フロー・チャートのＳ３１，Ｓ３５の処理を示す、図１６と同様の説明図である。
【図３０】図１８の同位相補償変位算出部の処理を説明するブロック図である。
【図３１】図１０のＳ３５の処理を示すサブルーチン・フロー・チャートで、同様に図１８の同位相補償変位算出部の処理を説明するフロー・チャートである。
【図３２】図３０で使用する重みの設定例を示す説明図である。
【図３３】図３２と同様の、重みの別の設定例を示す説明図である。
【図３４】２足歩行ロボットが予期しなかった傾斜面を歩行するときの説明図である。
【図３５】図３４に示す２足歩行ロボットに対して先に提案した両脚コンプライアンス制御を行った場合の説明図である。
【図３６】図３４に類似する、２足歩行ロボットが予期しなかった突起を踏んだときの説明図である。
【図３７】図３６に示す状況で、先に提案した足首コンプライアンス制御を行ったときの説明図である。
【符号の説明】
１ ２足歩行ロボット（脚式移動ロボット）
２ 脚部リンク
１０，１２，１４Ｒ，Ｌ 股関節
１６Ｒ，Ｌ 膝関節
１８，２０Ｒ，Ｌ 足関節
２２Ｒ，Ｌ 足平（足部）
２４ 上体
２６ 制御ユニット
３２ ばね機構
３４ 足底弾性体
４４ ６軸力センサ
６０ 傾斜センサ[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a control device for a legged mobile robot, and more particularly to a posture control device for the legged mobile robot. It relates to the one that appropriately controls the reaction force.
[0002]
[Prior art]
The most basic and simple legged mobile robot, more specifically, a control device for a biped walking robot is composed of a target motion pattern generation device and a joint drive control device. The target motion pattern generation device generates at least a target motion pattern. Usually, the walking motion pattern is generated by dynamic calculation, that is, the ZMP trajectory obtained by solving the Euler-Newton equation becomes a desired trajectory set in advance. The joint drive control device controls each joint so as to follow the displacement command of each joint generated by the gait generator.
[0003]
Here, ZMP (Zero Moment Point) means a point of action on the floor surface where the moment of the resultant force of the inertial force and gravity generated by the motion pattern is zero except for the component around the vertical axis.
[0004]
In this device, although the gait generating device generates a gait assuming a flat floor surface, as shown in FIG. 34, at the beginning of the both-leg support period, If the foot steps on an unexpected road surface, an excessive floor reaction force greater than expected on the foot is generated and the robot tilts. In order to solve the problem, the present applicant has proposed such a control device for a bipedal legged mobile robot in, for example, Japanese Patent Laid-Open No. 5-305586.
[0005]
In this case, the required amount of restoring moment required to restore the body posture by detecting the body inclination is obtained, and the actual total floor reaction force moment component around the target total floor reaction force central point (target ZMP) is obtained. Each foot is controlled to rotate up and down and rotate to detect it and make it coincide with the required amount of restoring moment. This actual total floor reaction force moment is a moment generated by the resultant force of each actual foot floor reaction force around the target total floor reaction force center point (target ZMP).
[0006]
The previously proposed control (hereinafter referred to as “both leg compliance control”) will be described taking as an example the case where there is an unexpected inclination as shown in FIG. For the sake of explanation, each foot is numbered as shown in this figure. Although the gait generator has generated a gait on the assumption of a flat floor surface, as shown in FIG. 34, the slope where the first foot was not expected at the beginning of the both-leg support period at present. It is assumed that this is the moment when a foot floor reaction force larger than a desired value is generated in the first foot. Further, it is assumed that the robot is still in a desirable posture (body tilt 0).
[0007]
In the proposed control device, the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) is detected. At this moment, the actual total floor reaction force moment acts in the direction of causing the robot to fall over later because the vertical component of the first foot floor reaction force is excessive.
[0008]
In order to reduce this moment to zero, as shown in FIG. 35, assuming the virtual floor surface AA ′, the virtual floor surface is placed on the virtual floor surface and the virtual floor surface is placed on the target total floor reaction force center point. The position of each foot is moved to a position rotated by an appropriate angle Δθ around (target ZMP).
[0009]
Thereby, the vertical component of the first foot floor reaction force decreases and the vertical component of the second foot floor reaction force increases. As a result, the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) becomes substantially zero. That is, even if there is an unexpected slope on the floor, both-leg compliance control works normally, so that the robot can continue walking without falling.
[0010]
However, since this proposed technology alone cannot control the actual floor reaction force of each foot during the support period for both legs, if there is an unexpected local inclination or unevenness in the floor shape around the foot contact point, the foot The grounding ability of the robot may decrease, making it easier to spin, or causing a sudden change in posture and falling.
[0011]
For example, as shown in FIG. 36, if the toe of the first foot steps on an unexpected protrusion (step) during the support period of both legs, the toe of the first foot will rapidly decrease during the support period of both legs. Since it is a certain time, the floor is strongly kicked by the toe, and the vertical component of the first foot floor reaction force increases rapidly. As a result, an actual total floor reaction force moment abruptly occurs around the target total floor reaction force center point (target ZMP), and in the worst case, even if an attempt is made to restore the posture by both-leg compliance, the vehicle falls over in time.
[0012]
In addition, even if the body does not fall during the period when both legs are supported, when the second foot is released from the floor immediately after that, the target total floor reaction force center point (target ZMP) is on the heel of the first foot. Because the heel is floating, the actual total floor reaction force center point is in a pinch, so an actual total floor reaction force moment is generated that tries to tilt the robot later around the target total floor reaction force center point (target ZMP). And falls.
[0013]
That is, it can be said that this both-legs compliance control can cope with the global inclination and the undulation that change slowly over a long distance, but cannot cope with the local inclination and the step of the landing point of the foot.
[0014]
Apart from the above-mentioned both-leg compliance control, the present applicant, for example, in Japanese Patent Application Laid-Open No. 5-3055584, is provided with a landing shock absorbing mechanism having a spring characteristic such as rubber at the ankle portion of a biped walking robot, We have proposed ankle compliance control in which each ankle is rotated to detect the actual foot floor reaction force moment component around it and make it zero.
[0015]
In order to solve the above problems, in addition to the both-leg compliance control, the technique proposed in Japanese Patent Laid-Open No. 5-305584 (hereinafter referred to as “ankle compliance control”) can be used in combination.
[0016]
As a result, as shown in FIG. 37, the ankle compliance control can rotate the first ankle in the direction to cancel the unexpected first foot floor reaction force moment, and contact the heel with the heel. Therefore, the robot does not fall down as described above even in the subsequent one leg support period.
[0017]
[Problems to be solved by the invention]
However, simply using both the leg compliance control and the ankle compliance control described above causes the two types of controls to interfere with each other, causing the actual total floor reaction force and the actual each floor floor reaction force to deviate or oscillate from the desired values. was there.
[0018]
Accordingly, an object of the present invention is to eliminate the above-mentioned inconvenience, and a legged mobile robot that can easily and appropriately control the actual floor reaction force acting on the legged mobile robot without causing interference. It is to provide a control device.
[0019]
Furthermore, when the robot is walking, an unexpected step, for example, when the actual floor surface is lower than the planned floor surface, the acceleration of the upper body (substrate) becomes excessive and the landing impact increases. Sometimes. The robot also has a natural vibration that is determined from its mechanism and mass and is displaced in the vertical direction. The vibration is slight as a displacement, but in some cases, the grounding property is lowered. Therefore, it is desirable to perform compliance control on the force component of the actual floor reaction force received by the robot.
[0020]
Accordingly, a second object of the present invention is to control a legged mobile robot which performs compliance control on the force component of the actual floor reaction force received by the robot to absorb the landing impact and improve the ground contact. To provide an apparatus.
[0021]
A third object of the present invention is to provide a legged mobile robot that is not greatly affected by unexpected floor shape changes including not only global undulation and inclination but also local unevenness and inclination. An object of the present invention is to provide a control device for a legged mobile robot capable of appropriately controlling an acting floor reaction force.
[0022]
A fourth object of the present invention is to provide a control apparatus for a legged mobile robot which can facilitate posture stabilization control of the legged mobile robot by further controlling the floor reaction force acting on the legged mobile robot. It is to provide.
[0023]
The fifth object of the present invention is to further improve the ground contact of the legged mobile robot by appropriately controlling the floor reaction force acting on the legged mobile robot, thereby preventing slipping and the above-mentioned spin during walking. It is an object of the present invention to provide a control device for a legged mobile robot.
[0024]
A sixth object of the present invention is to provide a control apparatus for a legged mobile robot that can reduce the load on the actuator of the legged mobile robot by appropriately controlling the floor reaction force acting on the legged mobile robot. There is to do.
[0025]
[Means for Solving the Problems]
In order to achieve the above object, according to claim 1, at least a base and a foot connected to the base via a first joint and connected to the tip thereof via a second joint. In a control device for a legged mobile robot comprising a plurality of legs having a portion, a target position of at least the foot of the robot Or A gait generating means for generating a gait of the robot including at least a motion pattern including a posture and a target pattern of a total floor reaction force acting on the robot; and the total floor reaction force of the generated gait Target foot floor reaction force center point determining means for determining a target foot floor reaction force center point as an action center point on the foot when distributed to each of the foot, and detecting an actual floor reaction force acting on the foot Actual floor reaction force detecting means, About the determined target foot floor reaction force center point The detected actual floor reaction force Made Calculate a moment to use, and rotate the foot based on at least the calculated moment Foot A foot rotation amount determining means for determining a rotation amount; a force component of an actual total floor reaction force obtained from the detected actual floor reaction force and a deviation of the generated gait from a force component of the total floor reaction force; And calculating a compensation displacement of the foot based on the calculated deviation. Decision Compensating displacement Decision Means, the position of the foot based on the determined foot rotation amount Put Or attitude The rotation Foot position / posture correction means for correcting the target position or posture of the foot by moving the target position or posture of the foot based on the determined compensated displacement, And before Note Part position / posture Correction Modified by means Said Foot Goal position Or Previous based on posture No. Joint displacement means for displacing the first and second joints;
It comprised so that it might be equipped with.
[0026]
According to a second aspect of the present invention, there are provided a plurality of legs each having at least a base and a foot connected to the base via a first joint and connected to the tip thereof via a second joint. In a control device for a legged mobile robot comprising a portion, a target position of at least the foot of the robot Or A gait generating means for generating a gait of the robot including at least a movement pattern including a posture and a target pattern of a total floor reaction force acting on the robot; and the total floor reaction force of the generated gait Target foot floor reaction force center point determining means for determining a target foot floor reaction force center point as an action center point on the foot when distributed to each of the foot, and detecting an actual floor reaction force acting on the foot An actual floor reaction force detecting means, a foot rotation amount determining means for determining a rotation amount for rotating the foot portion based on at least the detected actual floor reaction force, and an actual whole floor from the detected actual floor reaction force The force component of the reaction force is obtained and the deviation of the generated gait from the force component of the total floor reaction force is calculated, and the compensation displacement of the foot is calculated based on the calculated deviation. Decision Compensating displacement Decision Means, the position of the foot based on the determined amount of foot rotation Put Or attitude The , The determined target foot floor reaction force central point Also Rotates around its neighborhood A foot position / posture correction means for correcting the target position or posture of the foot by moving the target position or posture of the previous foot based on the determined compensation displacement, And before Note Part position / posture Correction Modified by means Said Foot Goal position Or Previous based on posture No. The joint displacement means for displacing the first and second joints is provided.
[0027]
In claim 3, the front Note The position / posture correction means further The position of the foot based on the determined amount of foot rotation. Put Or the posture is the determined target foot floor reaction force center point Also Said to rotate around its vicinity Foot Goal Put Or configured to correct the posture.
[0028]
5. The total floor reaction force moment actually acting on the robot or the floor reaction force moment acting on the foot is subtracted from the total floor reaction force moment actually acting on the robot. One of the obtained moments is calculated, and the foot is moved according to at least the calculated moment. Foot Provided with foot movement amount determining means for determining the movement amount ,Previous The foot position / posture correction means may determine the determined foot rotation amount. The determined compensation displacement And the determined Foot Based on the amount of movement Goal Place Put Or configured to correct the posture.
[0029]
According to a fifth aspect of the present invention, a posture stabilization compensation total floor reaction force moment to be added to the target pattern of the total floor reaction force is obtained, and the foot rotation amount determining unit is determined. Stepped Alternatively, the foot movement amount determination means is configured to determine the foot based on at least the detected actual floor reaction force and the obtained posture stabilization compensation total floor reaction force moment. Part-time Roll Amount Or Foot The movement amount is determined.
[0030]
According to a sixth aspect of the present invention, the posture stabilization compensation total floor reaction force moment is determined based on at least the tilt deviation of the robot.
[0031]
According to a seventh aspect of the present invention, the compensated displacement calculating means is a base acceleration detecting means for detecting a base acceleration of the robot, and the robot based on the detected base body acceleration. The The force component of the compensation floor reaction force to stabilize Decision Compensating total floor reaction force component Decision Means comprising Decision Compensated whole floor anti Power The compensation displacement of the foot is calculated based on the difference obtained by subtracting the deviation from the force component. Decision It was configured as follows.
[0032]
The compensation displacement according to claim 8. Decision The means is configured to multiply the difference by a predetermined weight and distribute the compensation displacement of the foot to each of the foot.
[0033]
The compensation displacement according to claim 9. Decision The means is configured such that the weight is set to zero when the foot is getting out of bed.
[0034]
The compensation displacement according to claim 10. Decision The means is configured to make the weight variable according to the frequency of the force component of the actual total floor reaction force.
[0035]
The compensation displacement according to claim 11 Decision Means for compensating the displacement of the foot in the direction of gravity Decision It was configured as follows.
[0036]
The compensation displacement according to claim 12. Decision The means determines the compensated displacement of the foot with respect to the direction of the line segment connecting the center of gravity and the target total floor reaction force center point. Decision It was configured as follows.
[0037]
According to the thirteenth aspect, the force component of the compensating total floor reaction force is set to zero or the vicinity thereof.
[0038]
[Action]
According to the first aspect, the floor reaction force acting on the legged mobile robot can be easily and appropriately controlled without causing interference. In other words, even if the control similar to the combined use of the two-leg compliance control and the ankle compliance control previously proposed is performed, there is no control interference, and the actual total floor reaction force and each actual floor reaction force deviate from desired values or oscillate. There is nothing to do.
[0039]
In addition, the legged mobile robot's posture stabilization control can be easily realized, the landing impact received by the legged mobile robot can be reduced, the grounding performance of the legged mobile robot can be improved, and slip and spin during walking can be reduced. Can be prevented.
[0040]
Furthermore, the floor reaction force acting on the legged mobile robot without being affected by unexpected changes in the floor shape, including not only global undulations and inclinations but also local irregularities and inclinations. It can be controlled appropriately. In addition, the load on the actuator of the legged mobile robot can be reduced.
[0041]
In the second aspect, the same effect as that of the first aspect can be obtained.
[0042]
According to the third aspect, the same effect as that of the first aspect can be obtained, and the floor reaction force can be more appropriately controlled.
[0043]
According to the fourth aspect, the same effect as that of the first aspect can be obtained, and the total floor reaction force particularly important for posture control can be controlled more appropriately.
[0044]
According to the fifth aspect, the same effect as that of the first aspect can be obtained, and the posture stabilization ability can be improved.
[0045]
According to the sixth aspect, the same operational effects as described above can be obtained, and the posture stabilization ability can be further improved.
[0046]
According to the seventh aspect, the same operational effects as described above can be obtained, the landing impact can be absorbed more effectively, and the ground contact can be further improved.
[0047]
In the eighth aspect, the same effect as described above can be obtained.
[0048]
According to the ninth aspect, the same effect as described above can be obtained, and the load on the actuator can be further reduced by reducing the control to a necessary limit.
[0049]
According to the tenth aspect, the same operational effects as described above can be obtained, and a more stable posture can be obtained.
[0050]
According to the eleventh aspect, the same operational effects as described above can be obtained, and the calculation amount can be reduced.
[0051]
In the twelfth aspect, the same operational effects as described above can be obtained, and the amount of calculation can be similarly reduced.
[0052]
According to the thirteenth aspect, the same operational effects as described above can be obtained, the landing impact can be absorbed and the ground contact can be improved to a considerable extent, and the configuration can be simplified. Can do.
[0053]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, a legged mobile robot controller according to the present invention will be described with reference to the accompanying drawings. As a legged mobile robot, a biped walking robot is taken as an example.
[0054]
FIG. 1 is a schematic diagram showing the control apparatus for the legged mobile robot as a whole.
[0055]
As shown in the figure, the bipedal walking robot 1 has six joints on the left and right leg links 2 (for convenience of understanding, each joint is shown by an electric motor that drives it). The six joints are, in order from the top, joints 10R, 10L for leg rotation of the crotch (waist) (R on the right side and L on the left side, the same applies hereinafter), the roll direction of the crotch (waist) (around the X axis) Joints 14R, 14L, joints 12R, 12L in the crotch (waist) pitch direction (around the Y axis), joints 16R, 16L in the knee pitch direction, joints 18R, 18L in the ankle pitch direction, joints in the roll direction 20R, 20L.
[0056]
Foot feet (foot portions) 22R and 22L are attached to the lower portions of the joints 18R (L) and 20R (L), and an upper body (base body) 24 is provided at the uppermost position. The control unit 26 (described later) is stored. In the above, the hip joint (or waist joint) is composed of joints 10R (L), 12R (L), 14R (L), and the ankle joint (ankle joint) is composed of joints 18R (L), 20R (L). The hip joint and knee joint are connected by thigh links 28R and 28L, and the knee joint and ankle joint are connected by crus links 30R and 30L.
[0057]
With the above configuration, the leg link 2 is given six degrees of freedom for the left and right feet, and by driving these 6 * 2 = 12 joints at an appropriate angle during walking, it is desired for the entire foot. Can be arbitrarily walked in a three-dimensional space (in this specification, “*” indicates multiplication for a scalar operation and outer product for a vector operation).
[0058]
Note that the position of the upper body and its speed described later in this specification mean a predetermined position of the upper body 24, specifically, the position of a representative point such as the position of the center of gravity of the upper body 24 and its moving speed.
[0059]
As shown in FIG. 1, a known six-axis force sensor 44 is attached below the ankle joint to measure three-direction components Fx, Fy, Fz of force and three-direction components Mx, My, Mz of moment, The presence or absence of landing on the foot and the floor reaction force (ground load) are detected.
[0060]
Further, an inclination sensor 60 is installed on the body 24 to detect an inclination with respect to the Z axis (vertical direction (gravity direction)) and its angular velocity. The tilt sensor 60 includes a G sensor and an angular velocity sensor, and the G sensor detects accelerations of the upper body 24 in the X, Y, and Z axis directions. Each electric motor of each joint is provided with a rotary encoder that detects the amount of rotation.
[0061]
As shown in FIG. 2, a spring mechanism 32 is provided above the foot 22R (L), and a sole elastic body 34 made of rubber or the like is attached to the sole. Specifically, the spring mechanism 32 is attached to the rectangular guide member attached to the foot 22R (L), the ankle joint 18R (L) and the six-axis force sensor 44, and an elastic material is applied to the guide member. And a piston-like member that is housed so as to be freely movable.
[0062]
A foot 22R (L) indicated by a solid line in the figure indicates a state when the floor reaction force is not received. When the floor reaction force is received, the spring mechanism 32 and the sole elastic body 34 bend, and the foot moves to the position / posture indicated by the dotted line in the figure. This structure is important not only for mitigating landing impacts but also for improving controllability. The details are described in the above-mentioned Japanese Patent Laid-Open No. 5-305584, and detailed description thereof is omitted.
[0063]
Further, although not shown in FIG. 1, a joystick 62 is provided at an appropriate position of the biped walking robot 1, and a request for a gait such as turning a robot that advances straight from the outside as needed is input. Configured to be able to.
[0064]
FIG. 3 is a block diagram showing the details of the control unit 26, which is composed of a microcomputer. The output from the tilt sensor 60 and the like is converted into a digital value by the A / D converter 70, and the output is sent to the RAM 74 via the bus 72. The output of the encoder arranged adjacent to each electric motor is input into the RAM 74 via the counter 76.
[0065]
The control unit is provided with first and second arithmetic devices 80 and 82 composed of a CPU. The first arithmetic device 80 is based on the gait stored in the ROM 84, as will be described later, as will be described later. A joint angle displacement command is calculated and sent to the RAM 74. The second arithmetic unit 82 reads out the command and the detected actual value from the RAM 74, calculates a control value necessary for driving each joint, and sets each joint via the D / A converter 86 and the servo amplifier. Output to the electric motor to be driven.
[0066]
Here, terms used in this specification and drawings are defined (for terms that are not defined, definitions used in an application (Japanese Patent Application No. 8-214261) proposed separately from the technique described above by the present applicant). Follow).
[0067]
Unlike the general definition in robot engineering, “gait” is used to refer to a combination of a target motion pattern and a floor reaction force pattern. However, the floor reaction force pattern may be partial information such as “ZMP locus only”. Therefore, the term “gait generator” is not used for a device that outputs only the target motion pattern and does not output information on the floor reaction force pattern.
[0068]
Each leg is given a serial number. The floor reaction force acting on the nth leg is referred to as the nth foot floor reaction force (n: 1 or 2, hereinafter the same). The total floor reaction force is a combination of floor reaction forces acting on all legs (generally called floor reaction forces in robotics, but here we will refer to them as “total floor reaction forces” to distinguish them from foot floor reaction forces. Called).
[0069]
The foot floor reaction force is expressed by the point of action, the force applied to it, and the moment of force. For the same foot floor reaction force, there are infinite combinations of expressions. Among them, there is a notation that the moment component excluding the component around the vertical axis is zero and the action point is on the floor surface. The action point in this expression is herein referred to as the foot floor reaction force center point (referred to as “ground pressure center of gravity point” in Japanese Patent Laid-Open No. 6-79657, which will be described later separately proposed by the present applicant).
[0070]
Similarly, the total floor reaction force is expressed by the point of action, the force applied thereto, and the moment of the force, and there are infinite combinations of expressions for the same total floor reaction force. Among them, there is an expression in which the moment component excluding the component around the vertical axis is 0 and the action point is on the floor surface. The action point in this expression is referred to herein as the center point of the total floor reaction force.
[0071]
The target value of the total floor reaction force is called the target total floor reaction force. The target total floor reaction force is usually a total floor reaction force that is dynamically balanced with respect to the target motion pattern. Therefore, the target total floor reaction force central point usually coincides with the target ZMP.
[0072]
As mentioned at the beginning, the target ZMP (Zero Moment Point) is defined as follows. That is, if the inertial force generated by the target motion pattern and the resultant force of gravity are determined kinetically and the moment acting on a certain point on the floor is zero except for the component around the vertical axis, that point is It is called a target ZMP (Zero Moment Point). The target ZMP is uniquely determined unless the vertical force component of the resultant force is zero. In the following explanation, the term target ZMP is sometimes used for easy understanding, but strictly speaking, the target ZMP is used. all There are many points that should be called the floor reaction force center point.
[0073]
The target value of each foot floor reaction force is referred to as a target foot floor reaction force. However, unlike the desired total floor reaction force, the desired foot floor reaction force is not uniquely determined even if the desired movement pattern is determined. The total floor reaction force acting on the actual robot is called the actual total floor reaction force. Each foot floor reaction force acting on an actual robot is called an actual foot floor reaction force.
[0074]
Here, when the subject of the present invention is re-explained, it is difficult for the present invention to obtain good posture stability by the two-leg compliance control previously proposed with respect to the local inclination and step, and the disadvantage is the ankle. Although compliance control can be used to solve this problem, simply using both of them interferes with each other, and the actual total floor reaction force and the actual foot floor reaction force deviate from desired values, or oscillate.
[0075]
The problem will be explained in the situation shown in FIG. 34. Since the first foot receives an unexpected excessive floor reaction force on the heel, an excessive actual foot floor reaction force around the first ankle. A moment is generated. In ankle compliance control, the first ankle is rotated as shown in FIG. 37 in order to reduce this moment.
[0076]
However, since the heel position of the first foot increases due to the rotation of the ankle, the vertical component of the first foot floor reaction force decreases. As a result, the actual total floor reaction force moment around the target total floor reaction force central point (target ZMP) changes. This means that the actual total floor reaction force moment, which is the control amount of the both-leg compliance control, is interfered with the ankle compliance control.
[0077]
Therefore, if the both-legs compliance control is operated in the same manner as without the ankle compliance control without considering the interference due to the ankle compliance control, the actual total floor reaction force moment around the target total floor reaction force central point (target ZMP) is 0. Or vibration or oscillation due to interference occurs.
[0078]
One way to prevent this is to calculate the amount of interference between the two-leg compliance control and the ankle compliance control, and to avoid interference by adding an operation amount that cancels it. However, it is extremely difficult to avoid interference with this method because the interference relationship changes every moment.
[0079]
In addition, in the situation shown in FIG. 37, the floor with which the first foot is in contact is climbing more inclined than the assumed floor, so the first foot should be better than the target gait. Nevertheless, it can be said that the ankle compliance control does not work properly if the ankle compliance control lowers the tow.
[0080]
As described above, ankle compliance control is effective for local floor inclinations and steps at the landing point of the foot, but it may adversely affect inclinations and undulations that change slowly over long distances. .
[0081]
Therefore, in the apparatus according to the embodiment, the floor reaction force acting on the legged mobile robot, more specifically, the actual total floor reaction force moment around the target total floor reaction force center point and the desired foot floor reaction. Each foot floor reaction force moment around the force center point can be controlled easily and appropriately.
[0082]
In addition to this, even if there is an unexpected floor shape change including not only global waviness and inclination but also local unevenness and inclination, the robot should continue to walk in a stable posture without being affected by it. I made it.
[0083]
In order to solve the above-mentioned problem, the present applicant has previously proposed a control device for a legged mobile robot, and the object of the present invention is to further improve the proposed technique and to provide a force component of the actual total floor reaction force. In particular, an object of the present invention is to provide a control device for a legged mobile robot which performs compliance control especially on the force component Fz in the direction of gravity to absorb the landing impact and improve the ground contact.
[0084]
FIG. 4 is a block diagram functionally showing the configuration and operation of the control device (mainly corresponding to the first arithmetic device 80 of FIG. 3) of the legged mobile robot according to this embodiment. The overall configuration of this apparatus will be outlined below with reference to FIG.
[0085]
The apparatus includes a gait generator, and the gait generator generates and outputs a target gait. The target gait is the target motion pattern and target floor reaction force pattern, more specifically, the target body position / posture trajectory, target foot position / posture trajectory, target total floor reaction force center point ( Target ZMP) trajectory and target total floor reaction force trajectory. Thus, the target floor reaction force pattern includes the target total floor reaction force center point locus (if the mechanism deformation compensation described later is not performed, the target floor reaction force center point locus alone is used as the target floor reaction force center pattern. good).
[0086]
In this embodiment, the desired total floor reaction force output by the gait generator is the total floor reaction force that is dynamically balanced with respect to the desired motion pattern. Accordingly, the target total floor reaction force central point coincides with the target ZMP.
[0087]
FIG. 5 shows an example of a target motion pattern when the robot 1 walks on a flat ground. The trajectory on the floor of the target ZMP trajectory corresponding to this is shown in FIG. 6, and the time chart is shown in FIG. The foot that is in contact with the floor during this gait period is referred to as the first foot, and the other foot as the second foot. The details of the gait generator are described in detail in the previously proposed Japanese Patent Application No. 8-214261, so further explanation is omitted.
[0088]
Returning to the explanation of FIG. 4, this apparatus includes a target floor reaction force distributor, and the target floor reaction force distributor mainly inputs a target total floor reaction force center point (target ZMP) and a target foot position / posture. The target foot floor reaction force center point is determined and output. Actually, from the gait generator, the gait parameters (for example, the time of both legs support period and the target landing position of the free leg foot) and the time and time of the gait (for example, the current time is the time of both legs support period) Information such as 0.1 sec from the beginning) is also taken in if necessary.
[0089]
For the gait as shown in FIG. 5, the desired floor reaction force distributor is set so that each desired foot floor reaction force center point satisfies the following conditions.
Condition 1) Target foot floor reaction force center point locus is continuous.
Condition 2) In the both-leg support period, the target first foot floor reaction force center point exists on the heel, and the target second foot floor reaction force center point exists on the heel.
Condition 3) At this time, the target total floor reaction force center point exists on a line segment connecting the target first foot floor reaction force center point and the target second foot floor reaction force center point.
Condition 4) In the one-leg support period, the target first foot floor reaction force central point coincides with the target total floor reaction force central point.
Condition 5) During the one-leg support period, the target second foot floor reaction force central point moves from the toe to the heel.
[0090]
FIG. 8 shows a time chart of the target first foot floor reaction force central point locus satisfying these conditions, and FIG. 9 shows a time chart of the target second foot floor reaction force central point locus. In this figure, the vertical projection point from the ankle (joint 18, 20R (L)) to the foot 22R (L) is the origin, and as shown in FIG. The flat left direction is the positive direction of the Y axis.
[0091]
The target floor reaction force distributor further determines and outputs each foot floor reaction force, although it is incidental. Each desired foot floor reaction force is necessary to compensate for deflection of the spring mechanism 32 and the like.
[0092]
If the target floor reaction force corresponding to the target foot floor reaction force center point set as described above is determined using the following equation, the resultant force of the target foot floor reaction force matches the target total floor reaction force. Satisfy the condition of having to.
[0093]
Figure 0003629142
[0094]
Since each desired foot floor reaction force obtained in this way changes continuously, it is suitable for realizing walking with less impact. The details described above are described in a technique (Japanese Patent Laid-Open No. 6-79657) separately proposed by the present applicant.
[0095]
Returning to the description of FIG. 4, this apparatus includes a posture stabilization control calculation unit, which estimates the state of the robot based on the sensor information of the robot and calculates a compensated total floor reaction force. That is, when the robot is actually walking or standing upright, even if the actual joint displacement can be made to completely follow the target joint displacement by the displacement controller described later, the position / posture of the robot is not necessarily the desired position / posture. I can't take a posture.
[0096]
To stabilize the robot's posture in the long term, obtain the force and moment necessary to restore the robot to the desired position and posture, and use this as the target point of the total floor reaction force center (target ZMP). It must be generated additionally. This additional force and moment is called compensated total floor reaction force. The moment component of the compensated total floor reaction force is referred to as a compensated total floor reaction force moment.
[0097]
It is assumed that the target gait of the legged mobile robot receives a reaction force other than the floor reaction force from the environment, which is referred to as a target object reaction force, for example, and the definition of the target ZMP described above is as follows: You may extend as follows. That is, the resultant force of the inertial force, gravity and target object reaction force generated by the target motion pattern is obtained dynamically, and the moment that acts on a certain point on the floor surface is zero except for the component around the vertical axis. If so, this point may be set as the target ZMP again.
[0098]
If it is assumed that the robot 1 is a completely rigid body, and the actual joint displacement can be made to follow the target joint displacement completely by the displacement controller, the robot generated by the deflection of the foot spring mechanism 32 and the sole elastic body 34. The perturbative motion of the entire position / posture can be decomposed into the following six degrees of freedom.
[0099]
Mode 1) Rotation around the longitudinal axis about the target floor reaction force center point (target ZMP) (ie, tilting left and right)
Mode 2) Rotation around the left / right axis about the target floor reaction force center point (target ZMP) (ie, forward / backward tilt)
Mode 3) Rotation around the vertical axis around the target total floor reaction force center point (target ZMP) (ie, spin)
Mode 4) Parallel translational shaking
Mode 5) Horizontal translation
Mode 6) Vertical translation shaking
[0100]
Of these, the mode 4 and the mode 5 are generated when the foot spring mechanism 32 and the elastic body 34 are bent by receiving a shearing force in the front-rear and left-right directions. Since the spring mechanism 32 and the sole elastic body 34 are manufactured so as to have high rigidity in the shearing direction, this swing is extremely small and there is almost no adverse effect on walking.
[0101]
In the technique proposed previously, mode 1 and mode 2 were mainly handled. However, in the embodiment according to the present invention, mode 6 is also handled. Note that, for convenience of understanding, the previous proposed technique will be described, although it is redundant.
[0102]
The operation amount for controlling the mode 1 is a moment component around the longitudinal axis (X axis) of the compensating total floor reaction force. The operation amount for controlling the mode 2 is a moment component around the left / right axis (Y axis) of the compensating total floor reaction force. Therefore, only the longitudinal axial moment component and the lateral axial moment component need be obtained from the components of the compensated total floor reaction force. Since other components are not used in this embodiment, they may be zero.
[0103]
The following definition follows. That is, the moment component of the compensated total floor reaction force is referred to as a compensated total floor reaction force moment Mdmd (specifically, the compensated total floor reaction force moment Mdmd around the target total floor reaction force central point (target ZMP)). As shown in FIG. 5, the robot's forward direction is the X-axis, the left lateral direction is the Y-axis, the upward direction is the Z-axis, and the coordinate system with the point on the floor just below the ankle of the first foot as the origin is the support leg This is called the coordinate system, and unless otherwise specified, the position, force and moment are expressed in this coordinate system. Further, the X component of Mdmd is described as Mdmdx, the Y component as Mdmdy, and the Z component as Mdmdz. The tilt deviation of the body 24 (ie, the actual body tilt—the target body tilt) θerr is described as θerrx, the Y component as θerr, and the time differential values thereof as (dθerrx / dt) and (dθerry / dt). To do.
[0104]
Mdmdx and Mdmdy are determined, for example, by the following control law.
Figure 0003629142
Here, Kthx, Kthy, Kwx, and Kwy are body tilt stabilization control gains.
[0105]
The composite compliance operation determination unit described later works to make the actual total floor reaction force coincide with the resultant force of the target total floor reaction force and the compensated total floor reaction force.
[0106]
Returning to the description of FIG. 4, this apparatus is provided with actual foot floor reaction force detectors. Floor reaction force) is detected. Further, based on the actual displacement (or displacement command) detected by the joint encoder, the relative position / posture of each foot with respect to the coordinate system fixed to the upper body is calculated, and the detected value of the six-axis force sensor 44 is thereby calculated. Is converted into coordinates, and the actual foot floor reaction force expressed in the coordinate system fixed to the upper body is calculated, and then converted into the support leg coordinate system.
[0107]
This device is equipped with a robot geometric model (inverse kinematics calculation unit). When a body position / posture and a foot position / posture are input, the robot geometric model calculates joint displacements that satisfy them. . When the degree of joint freedom per leg is 6 as in the robot 1 in this embodiment, each joint displacement is uniquely determined.
[0108]
In this embodiment, the inverse kinematics solution equation is obtained directly, and each joint displacement is obtained simply by substituting the body position / posture and the foot position / posture into the equation. That is, the robot geometric model inputs the target body position / posture and the corrected target foot position / posture trajectory (corrected target foot position / posture trajectory with mechanism deformation compensation) corrected by the composite compliance action determination unit. From them, joint displacement commands (values) of 12 joints (10R (L), etc.) are calculated. Note that the joint angle displacement may be obtained by a Jacobian.
[0109]
This apparatus includes a displacement controller (same as the above-described second arithmetic unit 82). The displacement controller uses a joint displacement command (value) calculated by a robot geometric model (inverse kinematics arithmetic unit) as a target value. The follow-up control of the displacement of 12 joints of 1 is performed.
[0110]
The composite compliance operation determination unit described above corrects the desired foot position / posture trajectory so as to satisfy the following two requirements.
[0111]
Requirement 1) To control the position / posture of the robot, the actual total floor reaction force is caused to follow the resultant force of the compensated total floor reaction force (moment Mdmd) output from the posture stabilization control unit and the target total floor reaction force. When it is desired to control only the posture inclination of the robot, only the actual total floor reaction force horizontal moment component around the target total floor reaction force center point is caused to follow the compensated total floor reaction force moment Mdmd.
[0112]
Requirement 2) In order to secure the contact property of each foot, the absolute value of the actual foot floor reaction force moment around the desired foot floor reaction force center point is made as small as possible.
[0113]
As a supplementary note, the actual foot floor reaction force moment around the center point of the desired foot floor reaction force is normally set to 0 while the actual total floor reaction force is matched with the resultant force of the compensated total floor reaction force and the desired total floor reaction force. In many cases, it is physically impossible. Therefore, requirements 1) and 2) cannot be completely compatible and must be compromised in some respects.
[0114]
Based on the above, the operation of this apparatus will be described with reference to FIG. 10 flow chart (structured flow chart). In addition, the component of FIG. 4 apparatus which performs the process applicable to the left end of a figure is shown.
[0115]
First, in S10, the apparatus is initialized, and in S12, the process proceeds to S14 to wait for a timer interrupt. A timer interrupt is made every 50 ms, that is, the control period is 50 ms.
[0116]
Subsequently, the process proceeds to S16, where it is determined whether or not the gait is switched, that is, whether the support leg is switched. When the determination is negative, the process proceeds to S22. Proceeding to S20, the target gait parameter is set. As described above, the gait parameters are composed of motion parameters and floor reaction force parameters (ZMP trajectory parameters).
[0117]
Subsequently, the process proceeds to S22, and an instantaneous value of the target gait is determined. Here, “instantaneous value” means a value for each control cycle, and the desired gait instantaneous value is composed of a desired body position / posture, each desired foot position / posture, and a desired ZMP position. Here, “posture” means “orientation” in X, Y, Z space.
[0118]
Subsequently, the process proceeds to S24 to obtain a target foot floor reaction force central point. This is done as described in the description of the target floor reaction force distributor. Specifically, this is performed by obtaining a value at the current time t of each desired foot floor reaction force center point locus set as shown in FIGS.
[0119]
Then, it progresses to S26 and calculates | requires each desired foot floor reaction force. This is performed by calculating each desired foot floor reaction force using the equation 1 described in the explanation of the target floor reaction force distributor.
[0120]
Subsequently, the process proceeds to S28, and the state of the robot 1 such as the tilt of the upper body 24 is detected from the output of the tilt sensor 60 and the like. At this time, actual accelerations in the X, Y, and Z directions acting on the body 24 are detected. In order to solve the above-described problem, in this embodiment, the body acceleration (referred to as “Gbodyz”) in the Z direction will be described as an example. However, the following description is also valid in the X and Y directions.
[0121]
Subsequently, the process proceeds to S30, and compensated total floor reaction force moments Mdmdx and Mdmdy (around the target total floor reaction force central point (target ZMP)) for obtaining a posture from the state of the robot 1 and the like are obtained. Specifically, when the body inclination is detected, the compensated total floor reaction force moments Mdmdx and Mdmdy are calculated according to the above-described equation 2 in order to stabilize the posture.
[0122]
Subsequently, the process proceeds to S31 and a force component of the compensated total floor reaction force is calculated based on the body Z direction acceleration control law, which will be described later.
[0123]
Subsequently, the process proceeds to S32 to detect the actual foot floor reaction force. This is detected from the output of the six-axis force sensor 44 as described above.
[0124]
Subsequently, the process proceeds to S34, in which both leg compensation angles θdbv and each foot compensation angle θnx (y) are determined. This is an operation performed by the composite compliance operation determination unit described above.
[0125]
The operation of the composite compliance operation determination unit will be described. For convenience of explanation, it is assumed that the actual foot floor reaction force is acting on the first foot 22R (L) and the second foot 22L (R) as shown in FIG.
[0126]
Here, the vector Fnact represents the force component of the nth foot floor reaction force. Vector Mact represents the moment component of the nth foot floor reaction force. The direction of the vector Mact represents that a clockwise moment acts on the foot from the floor with respect to the direction.
[0127]
It is assumed that the desired total floor reaction force at this moment is as shown in FIG. Incidentally, the target total floor reaction force moment vector Msumref at the target total floor reaction force central point (target ZMP) is vertical (by definition, the target ZMP is a point where the horizontal component of the target total floor reaction force moment is zero. From).
[0128]
When this is distributed to each desired foot floor reaction force according to Equation 1, it is as shown in FIG. In the figure, a vector Fnref represents a force component of the target nth foot floor reaction force. The vector Mnref represents the moment component of the target nth foot floor reaction force. The expression of the direction of the vector Mnref is the same as that of Mnact.
[0129]
For the sake of explanation, it is assumed that the upper body posture is likely to fall back left.
[0130]
The posture stabilization control calculation unit described above calculates the compensated total floor reaction force moment Mdmd based on the detected body inclination deviation values θerrx and θerror of the robot 1. In this embodiment, since the spin around the vertical axis (Z axis) is not controlled, the vertical axis component of the compensated total floor reaction force moment Mdmd is zero. FIG. 14 shows the compensated total floor reaction force moment Mdmd corresponding to this state.
[0131]
In order to restore the posture, the horizontal component of the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) is obtained by adding the sum of the target total floor reaction force moment Msumref and the compensated total floor reaction force moment Mdmd. What is necessary is just to make it follow a horizontal component.
[0132]
On the other hand, at the target total floor reaction force central point (target ZMP), the horizontal component of the target total floor reaction force moment Msumref is zero. Accordingly, in order to restore the posture inclination of the front, rear, left and right, the horizontal component of the actual total floor reaction force moment around the target ZMP may be made to follow the horizontal component of Mdmd.
[0133]
In this embodiment, the composite compliance operation determination unit corrects the position / posture of the foot so as to satisfy the following requirements as much as possible.
Requirement 1) In order to stabilize and control the posture inclination of the robot, the horizontal direction (X, Y axis direction) component of the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) is The horizontal component of the reaction force moment Mdmd is made to follow.
Requirement 2) In order to secure the contact property of each foot, the absolute value of the actual foot floor reaction force moment around the desired foot floor reaction force center point is made as small as possible.
[0134]
However, as mentioned above, request 1) and request 2) cannot be made completely compatible and must be compromised in some respects.
[0135]
In this embodiment, the position / posture of the foot is corrected as follows.
1) A normal vector V of a plane including the target first foot floor reaction force center point Q1 and the target second foot floor reaction force center point Q2 and perpendicular to the horizontal plane is obtained. The magnitude of V is 1. V is shown in FIG.
[0136]
2) The coordinates of the target first foot floor reaction force center point Q1 are rotated by a certain rotation angle θdbv around the normal vector V around the target total floor reaction force center point (target ZMP). Let Q1 'be the point after the movement. Similarly, the coordinates of the target second foot floor reaction force center point Q2 are rotated by the rotation angle θdbv around the normal vector V around the target total floor reaction force center point (target ZMP). Let Q2 ′ be the point after the movement.
[0137]
This rotation angle θdbv is referred to as a both-leg compensation angle. A vector having a start point Q1 and an end point Q1 ′ is a vector Q1Q1 ′. Similarly, a vector having a start point Q2 and an end point Q2 ′ is set as a vector Q2Q2 ′. FIG. 16 shows Q1 ′ and Q2 ′.
[0138]
3) The target first foot is translated (substantially moved up and down) by the vector Q1Q1 ′ without changing the posture. Similarly, the target second foot is translated by the vector Q2Q2 ′ without changing the posture. Each foot of the target after movement is indicated by a thick line in FIG.
[0139]
4) Next, the target first foot is rotated about Q1 ′ by the rotation angle θ1x about the front-rear direction axis (X axis) and the rotation angle θ1y about the left-right direction axis (Y axis). Similarly, the target second foot is rotated by a rotation angle θ2x about the front-rear direction axis (X axis) about Q2 ′ and a rotation angle θ2y about the left-right direction axis (Y axis). The rotation angles θnx and θny are referred to as the nth foot X compensation angle and the nth foot Y compensation angle, respectively. Each target foot after rotation is indicated by a thick line in FIG.
[0140]
If the above compensation operation amount is not excessive, even if the ground pressure distribution changes, the ground contact area (area where the pressure on the sole surface is positive) does not change. In such a case, the spring mechanism 32 and the sole elastic body 34 attached to each foot are deformed in proportion to the compensation operation amount, and an actual foot floor reaction force corresponding to the deformation amount is generated. As a result, the relationship between the compensation operation amount and the change amount of the actual floor reaction force generated by the compensation operation has the following favorable characteristics.
[0141]
Characteristic 1) When only the leg compensation angle θdbv is operated to move the target foot position, the force component of the actual foot floor reaction force of the lowered foot increases, and the actual foot floor reaction force of the raised foot increases. The force component is reduced. At this time, the actual foot floor reaction force moments around the corrected desired foot floor reaction force center point hardly change.
[0142]
Characteristic 2) When only the nth foot X compensation angle is operated to rotate the target nth foot posture, the X component of the moment of the actual nth foot floor reaction force acting on the target nth foot floor reaction force center point Only changes, and other floor reaction force components change little. Similarly, when only the nth foot Y compensation angle is operated to rotate the target nth foot posture, only the Y component of the moment of the actual nth foot floor reaction force changes, and the other floor reaction force components are It changes only a little.
[0143]
Characteristic 3) When the both leg compensation angle θdbv, each foot X compensation angle and each foot Y compensation angle are operated simultaneously, the change amount of the actual foot floor reaction force is the sum of the change amount when each of them is operated independently. Become.
[0144]
Characteristic 1 and characteristic 2 indicate that these operations are independent, and it can be said that characteristic 3 indicates that these operations are linear.
[0145]
FIG. 18 is a block diagram showing the calculation processing of the composite compliance operation determining unit, and this work will be described with reference to FIG.
[0146]
Briefly, the compensation total floor reaction force moment distributor distributes the compensation total floor reaction force moment Mdmd. Next, from the actual foot floor reaction force and the distributed compensation total floor reaction force moment, etc., the above-mentioned compensation angles θdbv and θnx (y ).
[0147]
Next, the corrected desired foot position / posture calculation unit obtains the compensated foot position / posture (hereinafter referred to as the corrected target foot position / posture) by geometric calculation based on the determined various compensation angles. Finally, the corrected desired foot position / posture calculation unit including mechanism deformation compensation calculates the deformation amount of the spring mechanism 32 and the sole elastic body 34 that are expected to be generated by the desired foot floor reaction force, and cancels them. Correct the corrected foot position / posture.
[0148]
More specifically, the compensating total floor reaction force moment distributor distributes the compensating total floor reaction force moment Mdmd to both leg compensation moments Mdmddb and each foot compensation moment Mdmd1x, y, Mdmd2x, y. The both-leg compensation moment Mdmdb is a target value of a moment generated by the force component of each foot floor reaction force around the target total floor reaction force center point (target ZMP) by manipulating the both leg compensation angle (foot vertical amount) θdbv. .
[0149]
A component around the V direction of the both-leg compensation moment Mdmdb is described as Mdmddvv. The vector V is a vector defined in the description of the composite compliance operation determination unit. If a vector orthogonal to V and orthogonal to the vertical direction is U, the U-direction component Mdmddbu of the both-leg compensation moment Mdmddbu is set to zero. This is because the U-direction moment component of the floor reaction force cannot be generated even if the both-leg compensation angle θdbv is manipulated.
[0150]
In this embodiment, since the vertical component of the compensated total floor reaction force moment Mdmd is zero, the vertical component Mdmdbz of Mdmdb is also set to zero.
[0151]
The first foot compensation moment Mdmd1 is a moment that is desired to be generated around the target first foot floor reaction force center point by manipulating the first foot compensation angles θ1x and θ1y. The X component of the first foot compensation moment Mdmd1 is described as Mdmd1x, and the Y component is described as Mdmdly. The second foot compensation moment Mdmd2 is a moment that is desired to be generated around the desired second foot floor reaction force center point by manipulating the second foot compensation angles θ2x and θ2y. The X component of the second foot compensation moment Mdmd2 is described as Mdmd2x, and the Y component is described as Mdmd2y.
[0152]
The distribution is performed as follows, for example.
Figure 0003629142
[0153]
Here, Wdbx, Wdby, W1x, W1y, W2x, W2y, and Wint are distribution weight variables. Vx is the value of the X component of the vector V, and Vy is the value of the Y component of the vector V. Among these, Wint is for canceling out the total floor reaction force moment generated by manipulating both leg compensation angles by manipulating each foot compensation angle.
[0154]
FIG. 19 shows a block diagram of a compensating total floor reaction force moment distributor that performs the arithmetic processing of Expression 3 and Expression 4.
[0155]
FIG. 20 shows a setting example of distribution weight variables Wdbx, Wdby, W1x, W1y, W2x, W2y and Wint during walking. The pattern shown in FIG. 20 is desirably determined in consideration of the following points.
[0156]
Note 1) When both leg compensation angles and each foot compensation angle change discontinuously, excessive torque is generated at the joint. Therefore, the distribution weight variable is continuously changed in order to continuously change both leg compensation angles and each foot compensation angle.
[0157]
Note 2) The distribution weight variable is determined so that the actual floor reaction force moment generated by manipulating both leg compensation angles and each foot compensation angle is as close to the compensated total floor reaction force moment Mdmd as possible. .
[0158]
At this time, it is better to change the setting policy as shown below according to the situation such as standing upright or walking. In a situation where the V-direction component Mdmddbv and the foot compensation moments Mdmd1 and Mdmd2 of the both leg compensation moments can be faithfully generated in the actual foot floor reaction forces, such as when standing upright, the following settings are made.
[0159]
In this situation, in order to make the horizontal component of the actual total floor reaction force moment around the target total floor reaction force center point (target ZMP) coincide with the horizontal component of the compensated total floor reaction force moment Mdmd (that is, as described above). In order to satisfy the requirement 1 for the composite compliance operation determining unit, the weights should be set so as to satisfy both the formulas 5 and 6 as much as possible.
[0160]
Figure 0003629142
[0161]
If Expression 3 and Expression 4 are substituted for this, Expression 5 is converted to Expression 7, and Expression 6 is converted to Expression 8.
Figure 0003629142
[0162]
Even if Mdmdx and Mdmdy take arbitrary values, Equations 9, 10 and 11 need only be satisfied at the same time in order for Equation 7 and Equation 8 to be established equally.
Figure 0003629142
That is, in the above situation, the weight may be determined so as to satisfy the expressions 9, 10, and 11 simultaneously.
[0163]
During walking, even if the position of the foot is corrected by manipulating both leg compensation angle θdbv with Mdmddv as a target, the amount of actual floor reaction force moment generated may be insufficient as compared to Mdmddv. For example, as shown in FIG. 21, in a situation where the robot tilts backward in the early stage of both-leg support and the first foot has not yet landed, the actual floor reaction force changes even if the position of the first foot is lowered by θdbv. do not do.
[0164]
Similarly, even if the second foot angle is corrected by operating the second foot compensation angle θ2 with Mdmd2 as a target, the increase amount of the actual floor reaction force moment may be insufficient as compared with Mdmd2. For example, in the situation where the robot is tilted backward in the latter half of the both-leg support period as shown in FIG. 22, even if the heel of the second foot is lowered by θ2, the actual floor reaction force does not change.
[0165]
Therefore, even if the respective weights are set so as to satisfy the expressions 5 and 6, the increase amount of the actual total floor reaction force generated by the composite compliance control may not reach the compensated total floor reaction force moment Mdmd. In a situation where such a possibility is likely to occur, the value obtained by dividing the value on the left side of Equations 5 and 6 by the right side should be greater than 1.
[0166]
In FIG. 20, which is an example of setting the distribution weight variable at the time of walking, by setting Wint to 0, an actual total floor reaction force moment can be generated even if the both-leg compensation angle θdbv is operated as in the situation of FIG. Even if it disappeared, each foot compensation angle was manipulated to compensate for the shortage.
[0167]
Conveniently, as shown in FIG. 21, if the heel of the second foot is lowered as a result, the heel of the second foot tends to come into contact with the floor, so that the actual total floor reaction force moment can be obtained by manipulating the second foot compensation angle. Can be generated.
[0168]
In addition, an actual total floor reaction force moment is generated by manipulating the both-leg compensation angle θdbv when the vehicle is not tilted backward, but the heel of the second foot does not touch the floor, so the second foot compensation angle is manipulated. However, the actual total floor reaction force moment does not occur.
[0169]
That is, when the both leg compensation angle θdbv works effectively, each foot compensation angle does not work effectively, and when each foot compensation angle works effectively, the both leg compensation angle θdbv does not work effectively. The total amount of the actual floor reaction force moment generated by manipulating each foot compensation angle is substantially equal to the compensation total floor reaction force moment Mdmd.
[0170]
Depending on the situation, the total amount of actual floor reaction force moment generated by manipulating both leg compensation angles and each foot compensation angle may be larger than the compensation total floor reaction force moment Mdmd.
[0171]
However, even in this case, if Mdmd is a feedback operation amount for posture stabilization as in this embodiment, there is not much problem. This is because even if the magnitude of Mdmd is slightly different, it can generally be said to the control system, but only the open loop gain of the control system changes slightly, and the closed loop characteristics hardly change. .
[0172]
Note 3) In the one-leg support period, the absolute values of Wdbx and Wdby, which are distribution weight variables for both-leg compensation angles, are reduced. This is because in the one-leg support period, even if the compensation angle for both legs is changed, the foot that is not in contact with the foot only moves up and down unnecessarily, and the actual foot floor reaction force does not change.
[0173]
Note 4) When the force component of the desired foot floor reaction force is small in order to secure the ground contact property of the foot, the absolute value of the distribution weight variable for the foot compensation angle of the foot is made small. In particular, when the foot is far from the floor, even if the foot compensation angle of the foot is moved, the actual foot floor reaction force of the foot does not change. The absolute value of the distribution weight variable for the foot compensation angle of the foot should be reduced.
[0174]
Note 5) The direction of the actual total floor reaction force moment that can be controlled by manipulating both leg compensation angles is different from the direction of the actual total floor reaction force moment that can be controlled by manipulating each foot compensation angle.
[0175]
For example, the direction of the actual total floor reaction force moment generated by manipulating the both-leg compensation angle θdbv is always the V direction, and a component orthogonal to the V direction cannot be generated. On the other hand, the direction of the actual total floor reaction force moment that can be generated by manipulating each foot compensation angle is limited by the ground contact state of the foot.
[0176]
For example, when only the toe edge or only the heel edge is grounded, a moment cannot be generated in the edge line direction. In the both-leg support period, in consideration of this characteristic, the both-leg compensation angle and each foot compensation angle are operated without waste as much as possible.
[0177]
For example, the distribution weights Wdbx and Wdby for operating both leg compensation angles are determined as follows.
[0178]
Assuming that a vector having an X component of Wdbx, a Y component of Wdby, and a Z component of 0 is Wdb, Equation 3 is an inner product of the vectors Wdb and Mdmd. Therefore, it can be said that Mdmdb obtained by Equation 3 is obtained by decomposing Mdmd into a vector Wdb direction component and its orthogonal component, extracting only the vector Wdb direction component, and multiplying by the magnitude of the vector Wdb.
[0179]
FIG. 23 shows Mdmddbv in this case. This means that a feedback control system is configured to control the Wdb direction component of the actual total floor reaction force moment by manipulating both leg compensation angles. If the Wdb direction is orthogonal to the vector V, no matter how much the both-leg compensation angle is manipulated, the Wdb-direction component of the actual total floor reaction force moment does not occur, so this feedback control system simply manipulates the both-leg compensation angle. Just do it.
[0180]
Therefore, when the useless movement is reduced, the Wdb direction should coincide with the vector V direction or be as close as possible. If the Wdb direction component of the compensation total floor reaction force moment Mdmd is generated only by operating both leg compensation angles without depending on each foot compensation angle, the inner product of Wdb and V is set to 1. To do. If it is desired to rely on each foot compensation angle for a part, the inner product of Wdb and V is set to be smaller than 1.
[0181]
By the way, when the lateral width of the foot is narrow, the X component of the actual foot floor reaction force moment that can be generated by operating each foot compensation angle becomes smaller. In this case, Wdbx is set larger. The Wdb direction and the vector V direction do not coincide with each other, and the variation of the both-leg compensation angle increases, but the stability increases.
[0182]
Further details of the both-leg compensation angle determination unit will be described below. FIG. 24 is a block diagram of the calculation process of the both-leg compensation angle determination unit, and the both-leg compensation angle θdbv is calculated as shown in the figure.
[0183]
Referring to FIG. 24, F1act acting on the target first foot floor reaction force center point Q1 and F2act acting on the target second foot floor reaction force center point Q2 are around the desired total floor reaction force center point P. The moment Mf1f2act to be generated is calculated by the following equation.
[0184]
Figure 0003629142
Here, PQ1 is a vector whose start point is P and end point is Q1, and PQ2 is a vector whose start point is P and end point is Q2.
[0185]
Moreover, there is practically no problem even if the following equation is used instead of equation 12.
Mf1f2act = PQ1 * F1act + PQ2 * F2act + M1act + M2act ... Formula 12a
Expression 12a is an expression for calculating the actual total floor reaction force moment Act acting around the target total floor reaction force center point. Equation 12 is obtained by subtracting the actual foot floor reaction force moment acting around the target foot floor reaction force center point from the actual floor reaction force moment acting around the target floor reaction force center point. ing. The description of claim 4 is based on this.
[0186]
Next, the vector V direction component Mf1f2actv of Mf1f2act is extracted. This is obtained by the following equation using a vector inner product operation. The vector V is the V shown in FIG. 15 in the above description of the operation.
Figure 0003629142
[0187]
Next, Mf1f2actvfilter is obtained by passing Mf1f2actv through a low-pass filter.
[0188]
Next, the both-leg compensation moment V-direction component Mdmddbv is passed through the compensation filter, and is subtracted from Mf1f2actvfilt to obtain the deviation moment V-direction component Mdiffv.
[0189]
The compensation filter improves the frequency response characteristic of the transfer function from Mdmddbv to the actual total floor reaction force moment.
[0190]
Next, a both-leg mechanism deformation compensation angle θffdbv for canceling the influence of the deformation of the foot spring mechanism or the like on the both-leg compensation moment V direction component is obtained. This is so-called feedforward compensation.
[0191]
Specifically, a line that connects the target first foot floor reaction force center point Q1 and the target second foot floor reaction force center point Q2 using a mechanism compliance model that represents the relationship between the both-leg compensation moment V-direction component Mdmddbv and the deformation amount. What is necessary is just to obtain | require the deformation angle of a minute, and what reverse | inverted the polarity is made into the both-legs mechanism deformation compensation angle (theta) ffdbv.
[0192]
The both-leg mechanism deformation compensation angle θffdbv may be obtained approximately by the following equation.
Figure 0003629142
Here, α is a predetermined constant.
[0193]
Finally, the both-leg compensation angle θdbv is obtained by the following equation. Here, Kdb is a control gain, which is normally set to a positive value.
Figure 0003629142
[0194]
The n-th foot compensation angle determination unit will be described. FIG. 25 is a block diagram showing the calculation processing of the first foot X compensation angle determination unit therein, and the first foot X compensation angle θ1x is calculated as shown in the figure. To do. Although the description is omitted, the first foot Y compensation angle θ1y, the second foot X compensation angle θ2x, and the second foot Y compensation angle θ2y are similarly obtained. Here, only the algorithm for obtaining the first foot X compensation angle θ1x will be described.
[0195]
The first foot floor reaction force moment X component M1actx is passed through a low-pass filter to obtain M1actfiltx. The first foot compensation moment X component Mdmd1x is passed through a compensation filter and is subtracted from M1actfiltx to obtain a deviation moment Mdiff1x. Similar to the determination of the both-leg compensation angle, the compensation filter improves the frequency response characteristic of the transfer function from Mdmd1x to the actual total floor reaction force.
[0196]
Next, the first foot X mechanism deformation compensation angle θff1x for canceling the influence on the first foot compensation moment X component due to the deformation of the foot spring mechanism or the like is obtained in the same manner as the both-leg compensation angle determination. This is so-called feedforward compensation.
[0197]
Specifically, the first foot compensation moment X A mechanism compliance model representing the relationship between the direction component Mdmd1x and the amount of deformation may be used to determine the deformation angle of the first foot, and the reverse of the polarity may be used as the first foot X mechanism deformation compensation angle θff1x.
[0198]
The first foot X mechanism deformation compensation angle θff1x may be determined approximately by the following equation.
Figure 0003629142
Here, α1x is a predetermined constant.
[0199]
Finally, the first foot X compensation angle θ1x is obtained by the following equation. Here, K1x is a control gain, which is normally set to a positive value.
Figure 0003629142
It should be noted that the illustrated block diagram may be equivalently modified such as changing the processing order.
[0200]
Returning to FIG. 18 and continuing the description, the corrected target foot position / posture calculation unit performs both leg compensation angle θdbv, first foot X compensation angle θ1x, first foot Y compensation angle θ1y, and second foot X compensation. Based on the angle θ2x, the second foot Y compensation angle θ2y, and in-phase compensated displacement (described later), the target foot position / posture is corrected according to the foot position / posture correction method that extends the above-mentioned composite compliance operation, and is corrected. Get the desired foot position / posture.
[0201]
The mechanism deformation amount calculation unit obtains deformation amounts of the spring mechanism 32 and the sole elastic body 34 that are expected to be generated by the desired foot floor reaction force.
[0202]
The corrected target foot position / posture calculation unit including mechanism deformation compensation further corrects the corrected target foot position / posture so as to cancel the calculated mechanism deformation amount, and the corrected target foot position / posture including mechanism deformation compensation is determined. obtain.
[0203]
For example, when a mechanism deformation amount as shown in FIG. 26 is predicted, the corrected target foot position / posture with mechanism deformation compensation is corrected to the position / posture indicated by the solid line in FIG. That is, the foot position after the mechanism deformation compensation shown in FIG. 27 is deformed by receiving the desired foot floor reaction force. The corrected target foot position / posture with mechanism deformation compensation is calculated so as to match the position / posture.
[0204]
The mechanism deformation compensation is a control that cancels out the deviation of the actual foot position / posture caused by the deformation of the spring mechanism 32 and the sole elastic body 34 in a feedforward manner. Compared to the case without this control, the target step is further increased. It is possible to realize a walking that is close to your heart.
[0205]
Returning to the description of the flowchart of FIG. 10 based on the above, the compensation angle described above is determined in S34 as described above.
[0206]
FIG. 28 is a subroutine flow chart showing the work.
[0207]
Referring to FIG. 20, first, the vector V described above is obtained in S100, the process proceeds to S102, the distribution weight variable is set as shown in FIG. 20, and these values at the current time t are obtained. Subsequently, the process proceeds to S104, and the total floor reaction force moment Mdmd is distributed to the both leg compensation moments Mdmddbv and the respective foot compensation moments Mdmdnx (y) according to Expressions 3 and 4, and the process proceeds to S106 and the both leg compensation angles θdbv as described above. The process proceeds to S108, and each foot compensation angle θnx (y) is obtained.
[0208]
Returning to the description of the flow chart of FIG. 10, the process proceeds to S35 to determine the isotope compensation displacement, which will be described later.
[0209]
Subsequently, the process proceeds to S36, where a mechanism deformation compensation amount is calculated based on each desired foot floor reaction force, and the process proceeds to S38, where the desired foot position / posture is compensated for the compensation angles θdbv, θnx (y), and in-phase compensated displacement (described later). ) According to the mechanism deformation compensation amount, and a corrected target foot position / posture with mechanism deformation compensation is obtained.
[0210]
Next, the process proceeds to S40, a joint displacement command (value) is calculated from the body position / posture and the corrected foot position / posture with mechanism deformation compensation, and the process proceeds to S42 to calculate the actual joint displacement to the calculated joint displacement command (value). Servo control is performed, the process proceeds to S44, the time is updated by Δt, and the process returns to S14 to repeat the above processing.
[0211]
Here, the processing of S31 and S35 in the flowchart of FIG. 10 will be described.
[0212]
These detect the body acceleration and determine the force component (target value) Fdmd of the compensated total floor reaction force according to the control law described later so that it does not become too large or too small. In this control, the high frequency component of the force component of the actual total floor reaction force is moved close to the high frequency component of Fdmd.
[0213]
When this control is performed, for example, when the robot 1 falls due to an unexpected step during walking and the feet 22R or L strongly land on the floor surface, the feet 22R (L) are slightly lifted in the direction of gravity. Can absorb the landing impact.
[0214]
Further, the robot has natural vibrations determined by the elasticity of the vertical spring mechanism 32 and the elastic bottom 34 of the foot and the mass of the robot 1 during walking, and the upper body 24 vibrates up and down. Although this vibration is slight as a displacement, the Z-direction force component Fz of the actual foot floor reaction force varies greatly.
[0215]
When the amplitude of this vibration becomes excessive, the ground contact between the foot 22R (L) and the floor surface is lowered, and the robot 1 spins depending on the case. This control suppresses such vibration, prevents spin, and improves posture stabilization. Therefore, in this embodiment, as shown in the lower part of FIG. 4, a body Z direction acceleration control calculation unit is added to the previously proposed apparatus.
[0216]
The body Z-direction acceleration control calculation unit calculates the actual body acceleration Gbody detected by the tilt sensor 60 (more specifically, for example, the detected actual body vertical acceleration Gbodyz) from the body acceleration ( That is, the force component (target value) Fdmd of the compensated total floor reaction force is calculated so as not to deviate significantly from the target body acceleration (Gbodyref). This corresponds to the process of S31.
[0217]
In the following, Fdmd is calculated as follows, for example. That is, when the G component of Gbodyref is Gbodyrefx, the X component is Gbodyrefy, and the Z component is Gbodyrefz, the deviation Fdmdtmp between the target value and the detected value is calculated according to the following equation.
[0218]
Figure 0003629142
[0219]
Here, Kgx, Kgy, and Kgz are control gains (constants). Fdmdtmpx, Fdmdttmpy, and Fdmdttmp are X, Y, and Z components of the deviation Fdmdttmp, respectively. The The sum is taken as the deviation Fdmdtmp. Subsequently, the calculated Fdmdtmp is passed through a filter appropriately designed to improve the control characteristics, and the output is defined as Fdmd.
[0220]
Here, if the vector having the starting point at the center of the total floor reaction force and the end point at the center of gravity of the robot is (Jx, Jy, Jz), the ratio of Kgx, Kgy, Kgz described above coincides with Jx, Jy, Jz. Good to do. Then, Fdmd also becomes the direction of the vector (Jx, Jy, Jz).
[0221]
Therefore, even if Fdmd is additionally generated as a floor reaction force at the target total floor reaction force center point, Fdmd acts toward the center of gravity of the robot, so that a moment for rotating the robot 1 does not occur, and the robot 1 Does not affect posture tilt. If the translation amount of all the feet determined by Fdmd is small, Kgx and Kgy may be set to zero. Even if it does so, it is because it does not affect the attitude | position inclination of the robot 1 similarly.
[0222]
Next, the process of S35 will be described.
[0223]
Outlined with reference to FIG. 29, in order to manipulate the force component of the actual total floor reaction force, the corrected target first foot position / posture obtained by the processing up to S38 ignoring the in-phase compensated displacement in the figure ( 29) (shown by a thin line in FIG. 29) is translated by a certain movement amount vector L1 without changing the posture.
[0224]
At the same time, the corrected target second foot position / posture is also translated by a certain movement amount vector L2 without changing the posture. The corrected target first and second foot position / posture after movement are indicated by bold lines in FIG.
[0225]
This is the compliance control for the force component of the actual total floor reaction force, which is referred to as “in-phase parallel movement operation” in this specification, and the movement vector Ln is referred to as the n-th foot in-phase compensated displacement (n = 1, 2). .
[0226]
That is, as shown in the lower part of FIG. 18, an in-phase compensated displacement calculation unit is provided, and each foot in-phase compensated displacement is calculated from the force component (target value) of the compensated total floor reaction force and the force component of the actual total floor reaction force. The corrected target foot position / posture calculation unit calculates the corrected target foot position / posture including each foot in-phase compensated displacement.
[0227]
FIG. 30 is a block diagram showing the operation in more detail, and FIG. 31 is a flowchart explaining the operation of FIG.
[0228]
Hereinafter, with reference to FIG. 31, in S200, the force component Ftotalact of the actual total floor reaction force is obtained. That is, the force component Ftotalact of the actual total floor reaction force is calculated from the force component F1act of the actual first foot floor reaction force and the force component F2act of the actual second foot floor reaction force by the following equation (Equation 19).
Ftotalact = F1act + F2act. . . Equation 19
[0229]
Subsequently, the process proceeds to S202, the deviation Ftotalerr is obtained by subtracting the desired total floor reaction force (that is, the force component Fref of the desired gait) from Ftotalact, and the process proceeds to S204 to obtain Ftotalerrfilt through an appropriate filter.
[0230]
Next, in S206, the force component (target value) Fdmd of the compensation total floor reaction force is passed through the compensation filter, and Ftotalerrfilt is subtracted from the filter passing value to obtain a difference Ftotaldiff. This compensation filter improves the frequency response from Fdmd to the force component of the actual total floor reaction force.
[0231]
Next, in S208, the in-phase compensation displacement vector L is calculated by the following equation (Equation 20).
L = K1 * Ftotaldiff. . . . Equation 20
Here, K1 is a control gain (constant, scalar quantity).
[0232]
Next, the calculated vector L is distributed to each foot according to the following equation.
Figure 0003629142
Here, W11 and W12 are attenuators (weights), which are set, for example, as shown in FIG.
[0233]
W11 and W12 may always be 1, but when the foot 22R (L) is in the air to avoid unnecessary movement up and down after leaving the floor (in the illustrated example, the second leg is in the one-leg support period). (Side side) 0 should be used.
[0234]
Including the in-phase compensation displacement Ln thus obtained, a corrected desired foot position / posture with mechanism deformation compensation is calculated in S38 of the flowchart of FIG.
[0235]
Specifically, the both foot compensation angle θdbv, the first foot X compensation angle θ1x, the first foot Y compensation angle θ1y, the second foot X compensation angle θ2x, the second foot Y compensation angle θ2y, and the first foot Based on the in-phase compensation displacement L1 and the second foot in-phase compensation displacement L2, the corrected target foot position / posture is calculated according to the composite compliance operation including the in-phase parallel movement operation.
[0236]
In this embodiment, as described above, at least the base body (upper body 24) is connected to the base body via the first joint (10, 12, 14R (L)), and the second joint is connected to the tip thereof. Control of a legged mobile robot composed of a plurality of (two) legs (leg links 2) each having a foot (foot 22R (L)) connected via (18, 20R (L)). In the apparatus, a motion pattern (target body position / posture, target foot position / posture) including at least the target position and posture of the foot of the robot, and a target pattern (target target of the total floor reaction force acting on the robot) Gait generating means (gait generator, S10 to S22) for generating a gait of the robot including at least a total floor reaction force and a target total floor reaction force center point (= target ZMP), and the generated gait The total floor reaction force was distributed to each of the feet Target foot floor reaction force center point determining means (target floor reaction force distributor, target floor reaction force center point) that determines a target foot floor reaction force center point (a target foot floor reaction force center point) as an action center point on the mushroom foot S24, S26), actual floor reaction force detecting means for detecting the actual floor reaction force (actual foot floor reaction force) acting on the foot (six-axis force sensor 44, actual foot floor reaction force detector, S32). , Calculating a moment (actual nth foot floor reaction force moment Mactx, y, z) at which the detected actual floor reaction force acts around the calculated target foot floor reaction force center point, and at least the calculated Foot rotation amount determining means (composite compliance operation determining unit, S32) for determining a rotation amount (both leg compensation angle θdbv, z, n-th foot compensation angle θnx, y, z) for rotating the foot based on the moment. To S34, both leg compensation angle determination unit, nth foot compensation angle determination unit, S1 0 to S108), first foot position / posture correcting means for correcting the target position and / or posture so that the position and / or posture of the foot rotates based on the determined amount of foot rotation. (Composite compliance action determining unit, S38, S40, corrected target foot position / posture calculating unit), obtaining the force component Ftotalact of the actual total floor reaction force from the detected actual floor reaction force, and Compensation displacement calculation means (an upper body Z-direction acceleration control calculation unit, in-phase compensation) that calculates a deviation Ftotalerr from the force component Fref of the total floor reaction force and calculates a compensation displacement Ln of the foot based on the calculated deviation Displacement calculation unit, S31, S35, S200 to S208), second foot position / posture correction means (upper body Z direction) for correcting the target foot position / posture based on the calculated compensated displacement An acceleration control calculation unit, an in-phase compensated displacement calculation unit, S38) and a first position of the robot based on the position / posture of the foot corrected by the first and the first and second foot position / posture means. And a joint displacement means (robot geometric model (kinematics calculation unit), displacement controller, S40, S42) for displacing the second joint (10, 12, 14, 18, 20R (L)). .
[0237]
In addition, at least the base body (upper body 24) is connected to the base body via a first joint (10, 12, 14R (L)), and a second joint (18, 20R (L)) is connected to the tip thereof. In the control device for the legged mobile robot 1 composed of a plurality of (two) legs (leg links 2) having legs (foot 22R (L)) connected via A motion pattern (target body position / posture, target foot position / posture) including at least the target position and posture of the foot, and a target pattern (target total floor reaction force, target) of the total floor reaction force acting on the robot Gait generation means (gait generators S10 to S22) for generating a gait of the robot including at least the total floor reaction force central point (= target ZMP), and the total floor reaction force of the generated gait. Action on the foot when distributed to each of the foot Target foot floor reaction force center point determining means (target floor reaction force distributor, S24) for determining a target foot floor reaction force center point (target foot floor reaction force center point) as a center point, acting on the foot Actual floor reaction force detecting means (6-axis force sensor 44, actual foot floor reaction force detector, S32) for detecting actual floor reaction force (actual foot floor reaction force), at least the detected actual floor reaction force Foot rotation amount determining means (composite compliance operation determining unit, S32, S34) for determining a rotation amount (both leg compensation angle θdbv, z, nth foot compensation angle θnx, y, z) based on , Both leg compensation angle determination unit, n-th foot compensation angle determination unit, S100 to S108), the position and / or posture of the foot based on the determined foot rotation amount, the determined target foot The target position so as to rotate around or near the floor reaction force center point. And / or first foot position / posture correcting means for correcting posture (composite compliance action determining unit, S38, S40, corrected target foot position / posture calculating unit), and the detected actual floor reaction force A force component Ftotalact of the floor reaction force is obtained, a deviation Ftotalerr of the generated gait from the force component Fref of the total floor reaction force is calculated, and a compensation displacement Ln of the foot is calculated based on the calculated deviation. Compensation displacement calculation means (upper body Z-direction acceleration control calculation unit, in-phase compensation displacement calculation unit, S31, S35, S200 to S208), second to correct the target foot position / posture based on the calculated compensation displacement Foot position / posture correction means (upper body Z-direction acceleration control calculation section, in-phase compensated displacement calculation section, S38) and the first and first and second foot position / posture means. Joint displacement means (robot geometric model (kinematics) for displacing the first and second joints (10, 12, 14, 18, 20R (L)) of the robot based on the corrected position and posture of the foot. A calculation unit), a displacement controller, and S42) are provided.
[0238]
In addition, the first foot position / posture correction means may determine whether the foot position and / or posture is based on the determined foot rotation amount based on the determined foot rotation amount. The target position and / or posture is corrected so as to rotate around the vicinity.
[0239]
Further, the total floor reaction force moment actually acting on the robot (more precisely, moment component PQ1 * F1act + PQ2 * F2act + M1act + M2act), or the moment of the total floor reaction force actually acting on the robot (PQ1 * F1act + PQ2 * F2act + M1act + M2act) is calculated from any of the moments (Mf1f2act = PQ1 * F1act + PQ2 * F2act) obtained by subtracting the floor reaction force moment (M1act + M2act) acting on the foot. Foot movement amount determination means (composite compliance operation determination unit, S34, both leg compensation angle determination unit, S100 to S108) for determining a movement amount (θdbv, z) for moving the foot according to Foot position Posture correcting means configured to correct the position and / or posture of the foot based on the movement amount of the is determined foot rotating amount and the determined.
[0240]
Further, a posture stabilization compensation total floor reaction force moment (compensation total floor reaction force Mdmd) to be added to the target pattern of the total floor reaction force is obtained, and the foot rotation amount determination means and / or the foot movement amount determination means. Determines the amount of rotation and / or movement of the foot based on at least the detected actual floor reaction force (actual foot floor reaction force) and the calculated posture stabilization compensation total floor reaction force moment ( S34, S100 to S108).
[0241]
The posture stabilization compensation total floor reaction force moment is determined based on at least the tilt deviation (θerrx, y) of the robot (S28, S30).
[0242]
The compensation displacement calculating means stabilizes the posture of the robot based on the base body acceleration detecting means (S31) for detecting the base body acceleration (actual body vertical acceleration) Gbody of the robot and the detected base body acceleration. Compensation floor reaction force component calculation means (S31) for calculating a force component of the compensation floor reaction force (target value Fdmd) for subtracting the deviation from the calculated compensation floor reaction force component Based on the difference (Ftotaldiff or L) obtained as described above, the compensation displacement Ln of the foot is calculated (S200 to S208).
[0243]
Further, the compensation displacement calculating means is configured to multiply the difference (Ftotaldiff or L) by a predetermined weight (attenuator W11, W12) and distribute the compensation displacement of the foot to each of the foot.
[0244]
Further, the compensation displacement calculating means is configured to make the weight zero when the foot is getting out of bed.
[0245]
The compensation displacement calculating means is configured to make the weight variable according to the frequency of the force component of the actual total floor reaction force.
[0246]
The compensated displacement calculating means is configured to calculate the compensated displacement of the foot in the direction of gravity.
[0247]
The compensation displacement calculation means is configured to calculate the compensation displacement of the foot in the direction of the line segment connecting the center of gravity and the target total floor reaction force center point.
[0248]
Further, the force component of the compensating total floor reaction force is set to zero or in the vicinity thereof.
[0249]
Since it is configured as described above, in summary, the control of the actual total floor reaction force and the control of the actual foot floor reaction force hardly interfere with each other and can be easily controlled.
[0250]
That is, the apparatus according to this embodiment has the following improvements over the previously proposed technique. That is, in the ankle compliance control proposed in JP-A-5-305584, an actual floor reaction force moment at a point fixed to the foot such as the reference point of the ankle or the sole is detected, and the fixed is based on the detected moment. Although the foot is rotated around the point, the apparatus according to this embodiment calculates the actual foot floor reaction force moment at the moving target foot floor reaction force center point, and based on this, calculates the desired foot floor reaction force. Changed to rotate the foot around the center of force, and controlled the moment around that point to the desired value.
[0251]
As a result, the actual total floor reaction force and the actual foot floor reaction force can be easily controlled with little interference. In order to further reduce the interference, a more appropriate point may be selected within the assumed ground contact area at each moment.
[0252]
Therefore, the floor reaction force acting on the legged mobile robot is not affected by unexpected changes in the floor shape, including not only global swell and inclination but also local unevenness and inclination. Can be controlled.
[0253]
Furthermore, the floor reaction force acting on the robot, more specifically, the actual total floor reaction force moment about the target total floor reaction force center point (target ZMP) and the actual foot floor reaction force about each target foot center point. The moment can be easily and appropriately controlled. In other words, compared to the previously proposed combined use of both leg compliance control and ankle compliance control, there is no control interference, and the actual total floor reaction force and the actual foot floor reaction force may deviate or oscillate from desired values. Absent.
[0254]
In addition, because compliance control is performed for the force component of the actual floor reaction force received by the robot, it can absorb the landing impact even if it encounters an unexpected level difference and causes an excessive landing impact. At the same time, the grounding property can be improved and the occurrence of spin can be prevented.
[0255]
In addition, the legged mobile robot's posture stabilization control can be easily realized, the landing impact received by the legged mobile robot can be reduced, the grounding performance of the legged mobile robot can be improved, and slip and spin during walking can be reduced. Can be prevented. Furthermore, the load on the actuator of the legged mobile robot can be reduced.
[0256]
Here, supplementing the distribution of the in-phase compensation displacement L to each foot, in Equation 21, for example, the weights Wl1 and Wl2 are fixed to 1.0, or 0 in the case of a free leg, in other words, Fixed to the frequency of the force component of the actual total floor reaction force.
[0257]
If so set, the low frequency component of the force component of the actual total floor reaction force cannot be arbitrarily controlled. This is because, for example, even if both feet are moved in the same phase in the direction of gravity when standing upright (that is, both legs are contracted), the force component of the actual total floor reaction force decreases at that moment, but immediately after that. This is because the upper body 24 returns to the original actual floor reaction force again when it moves downward.
[0258]
If an integral term is added to Equation 20 to eliminate this, it is possible to control even the low frequency component of the actual total floor reaction force. However, during high-speed walking, if the body height changes greatly from the target gait, the dynamic balance is lost, so it is not preferable to add an integral term.
[0259]
Therefore, the distribution amount of the in-phase compensation displacement L may be different between both feet, or may be variable with respect to the frequency of the force component of the actual total floor reaction force. That is, a weight having a frequency component may be used.
[0260]
This will be described below. As shown in FIG. 33, the weights W11 and W12 are set variably with respect to the frequency of the force component of the actual total floor reaction force. Specifically, in a frequency range sufficiently lower than a predetermined value, for example, 5 Hz, the ratio between the weights Wl 1 and Wl2 is set near 1 and the ratio is set to change in a frequency range higher than that.
[0261]
For example, in a frequency region sufficiently lower than 5 Hz, L1: L2 = 1: 1 is determined. That is, for the low frequency component of the force of the actual total floor reaction force, the upper body 24 moves greatly in the direction of gravity following the expansion and contraction of the foot 22R (L), so that no foot floats. Set the same value for both feet.
[0262]
On the other hand, in the high frequency region of 5 Hz or more, in FIG. 29, the target total floor reaction force center point P and the corrected target nth footbed are prevented so that no moment is generated around the target total floor reaction force center point P. It is set so as to be inversely proportional to the distance from the reaction force center point Qn ′. More specifically, in FIG. 29, it is determined so that L1: L2 = PQ2 ′: PQ1 ′.
[0263]
In addition, you may generalize the above-mentioned Formula 18 as follows.
Figure 0003629142
However, Kgxx, Kgxy, Kgxz, Kgyx, Kgyy, Kgyz, Kgzzx, Kgzy, Kgzz are control gains.
[0264]
In the above-described embodiment, the control law for detecting the body acceleration detection and the force component (target value) Fdmd of the compensation floor reaction force is omitted, and the Fdmd is fixed to zero or in the vicinity thereof. It is quite effective as compliance control for the force component of floor reaction force. The twelfth aspect is based on this description.
[0265]
In the above-described embodiment, the spring mechanism 32 (and the sole elastic body 34) itself is not an essential part of the present invention. The essence of the present invention resides in the feedback control portion, and the mechanism deformation compensation is incidental.
[0266]
In the above-described embodiment, the block diagram may be subjected to equivalent modifications such as changing the calculation processing order.
[0267]
Moreover, although the above-described embodiment has been described with respect to the first embodiment of the previously proposed application, it may be added with respect to the second or later embodiments.
[0268]
Although the present invention has been described with respect to a biped robot, the present invention can be applied not only to a biped robot but also to a multi-legged robot.
[0269]
【The invention's effect】
According to the first aspect, the floor reaction force acting on the legged mobile robot can be easily and appropriately controlled without causing interference. In other words, even if the control similar to the combined use of the two-leg compliance control and the ankle compliance control previously proposed is performed, there is no control interference, and the actual total floor reaction force and each actual floor reaction force deviate from desired values or oscillate. There is nothing to do.
[0270]
In addition, the legged mobile robot's posture stabilization control can be easily realized, the landing impact received by the legged mobile robot can be reduced, the grounding performance of the legged mobile robot can be improved, and slip and spin during walking can be reduced. Can be prevented.
[0271]
Furthermore, the floor reaction force acting on the legged mobile robot without being affected by unexpected changes in the floor shape, including not only global undulations and inclinations but also local irregularities and inclinations. It can be controlled appropriately. In addition, the load on the actuator of the legged mobile robot can be reduced.
[0272]
In the second aspect, the same effect as that of the first aspect can be obtained.
[0273]
According to the third aspect, the same effect as that of the first aspect can be obtained, and the floor reaction force can be more appropriately controlled.
[0274]
According to the fourth aspect, the same effect as that of the first aspect can be obtained, and the total floor reaction force particularly important for posture control can be controlled more appropriately.
[0275]
According to the fifth aspect, the same effect as that of the first aspect can be obtained, and the posture stabilization ability can be improved.
[0276]
According to the sixth aspect, the same operational effects as described above can be obtained, and the posture stabilization ability can be further improved.
[0277]
According to the seventh aspect, the same operational effects as described above can be obtained, the landing impact can be absorbed more effectively, and the ground contact can be further improved.
[0278]
In the eighth aspect, the same effect as described above can be obtained.
[0279]
According to the ninth aspect, the same effect as described above can be obtained, and the load on the actuator can be further reduced by reducing the control to a necessary limit.
[0280]
According to the tenth aspect, the same operational effects as described above can be obtained, and a more stable posture can be obtained.
[0281]
According to the eleventh aspect, the same operational effects as described above can be obtained, and the calculation amount can be reduced.
[0282]
In the twelfth aspect, the same operational effects as described above can be obtained, and the calculation amount can be reduced.
[0283]
According to the thirteenth aspect, the same operational effects as described above can be obtained, the landing impact can be absorbed and the ground contact can be improved to a considerable extent, and the configuration can be simplified. Can do.
[Brief description of the drawings]
FIG. 1 is an explanatory diagram generally showing a control device for a legged mobile robot according to the present invention;
FIG. 2 is an explanatory side view showing a structure of a foot portion of the biped robot shown in FIG.
FIG. 3 is a block diagram showing details of a control unit of the biped walking robot shown in FIG.
FIG. 4 is a block diagram functionally showing the configuration and operation of a control device for a legged mobile robot according to the present invention.
FIG. 5 is an explanatory diagram showing an example of a motion pattern when the legged mobile robot shown in FIG. 1 walks on a flat ground.
6 is an explanatory diagram showing a trajectory on the floor surface of a target total floor reaction force central point (target ZMP) trajectory corresponding to the motion pattern of FIG. 5;
7 is a time chart of a target total floor reaction force central point (target ZMP) locus corresponding to the motion pattern of FIG. 5. FIG.
8 is a time chart of a target first foot floor reaction force center point locus set to satisfy a predetermined condition corresponding to the motion pattern of FIG. 5;
9 is a time chart of a target second foot floor reaction force central point locus set so as to satisfy a predetermined condition corresponding to the motion pattern of FIG. 5;
FIG. 10 is a flowchart showing the operation of the control device for the legged mobile robot according to the present invention, similar to FIG. 4;
FIG. 11 is a diagram illustrating the operation of the composite compliance operation determination unit shown in FIG. 4 that performs arithmetic processing such as the compensation angle of both legs in the flowchart of FIG. 10; It is explanatory drawing which shows the condition where real each foot floor reaction force is acting on a foot.
12 is an explanatory diagram showing setting of a target total floor reaction force in the situation shown in FIG. 11. FIG.
13 is an explanatory diagram showing distribution of desired foot floor reaction forces in the situation shown in FIG. 11. FIG.
14 is an explanatory diagram showing a compensated total floor reaction force moment in the situation shown in FIG. 11. FIG.
15 is an explanatory diagram showing a normal vector V of a plane including each foot floor reaction force center point and perpendicular to the horizontal plane in the situation shown in FIG.
16 is an explanatory diagram showing a state when the desired foot floor reaction force central point is rotated by a predetermined angle θdbv around the target total floor reaction force central point (target ZMP) in the situation shown in FIG. .
17 is an explanatory diagram showing a state in which each foot is rotated by a predetermined angle θnx, θny about the front-rear direction axis and the left-right direction axis in the situation shown in FIG. 16;
18 is a block diagram showing a calculation process of a composite compliance operation determination unit in FIG. 4. FIG.
FIG. 19 is a block diagram showing a calculation process of the compensated total floor reaction force moment distributor shown in FIG. 18;
20 is a time chart showing an example of setting distribution weight variables for operating both leg compensation angles and the like of the compensated total floor reaction force moment distributor shown in FIG. 18;
FIG. 21 is an explanatory diagram showing the posture of the robot for explaining the setting of the distribution weight variable of the compensated total floor reaction force moment distributor of FIG. 20;
FIG. 22 is an explanatory view showing the posture of the robot for explaining the setting of the distribution weight variable of the compensated total floor reaction force moment distributor as in FIG.
FIG. 23 is an explanatory diagram showing a both-leg compensation moment V-direction component Mdmddvv when a distribution weight for operating both-leg compensation angles is determined under a predetermined condition.
24 is a block diagram showing a calculation process of a both-leg compensation angle determination unit shown in FIG.
25 is a block diagram showing calculation processing of a compensation angle determination unit for each foot shown in FIG. 18;
26 is an explanatory diagram for explaining a calculation process of a corrected desired foot position / posture calculation unit with mechanism deformation compensation shown in FIG.
27 is an explanatory diagram for explaining the calculation processing of the corrected desired foot position / posture calculation unit with mechanism deformation compensation shown in FIG. 18 as in FIG. 26;
FIG. 28 is a subroutine flow chart showing a determination work such as a both-leg compensation angle in the flowchart of FIG. 10;
29 is an explanatory view similar to FIG. 16, showing the processing of S31 and S35 in the flowchart of FIG. 10 by the operations of the body acceleration control calculation unit of FIG. 4 and the in-phase compensated displacement calculation unit of FIG. .
30 is a block diagram illustrating processing of the in-phase compensated displacement calculating unit in FIG.
31 is a subroutine flowchart showing the process of S35 of FIG. 10, and similarly a flowchart explaining the process of the in-phase compensated displacement calculating unit of FIG.
32 is an explanatory diagram showing a setting example of weights used in FIG. 30. FIG.
33 is an explanatory view showing another example of setting weights, similar to FIG. 32. FIG.
FIG. 34 is an explanatory diagram when the biped walking robot walks on an inclined surface that was not anticipated.
FIG. 35 is an explanatory diagram when the previously proposed both-leg compliance control is performed on the biped robot shown in FIG. 34;
FIG. 36 is an explanatory view similar to FIG. 34 when the biped walking robot steps on an unexpected projection.
FIG. 37 is an explanatory diagram when the previously proposed ankle compliance control is performed in the situation shown in FIG. 36;
[Explanation of symbols]
1 Biped walking robot (legged mobile robot)
2 Leg links
10, 12, 14R, L Hip joint
16R, L Knee joint
18, 20R, L Ankle joint
22R, L Foot (foot)
24 upper body
26 Control unit
32 Spring mechanism
34 Sole elastic body
44 6-axis force sensor
60 Tilt sensor

Claims (13)

少なくとも基体と、前記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、
ａ．前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、
ｂ．前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、
ｃ．前記足部に作用する実床反力を検出する実床反力検出手段、
ｄ．前記決定された目標足部床反力中心点まわりに前記検出された実床反力が作用するモーメントを算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる足部回転量を決定する足部回転量決定手段、
ｅ．前記検出された実床反力から実全床反力の力成分を求めて前記生成された歩容の全床反力の力成分との偏差を算出し、前記算出した偏差に基づいて前記足部の補償変位を決定する補償変位決定手段、
ｆ．前記決定された足部回転量に基づいて前記足部の位置または姿勢を回転させつつ、前記決定された補償変位に基づいて前記足部の目標位置または姿勢を移動させることによって、前記足部の目標位置または姿勢を修正する足部位置・姿勢修正手段、
および
ｇ．前記足部位置・姿勢修正手段によって修正された前記足部の目標位置または姿勢に基づいて前記第１および第２の関節を変位させる関節変位手段、
を備えたことを特徴とする脚式移動ロボットの制御装置。Control of a legged mobile robot comprising at least a base body and a plurality of leg portions each having a foot portion connected to the base body via a first joint and connected to the tip thereof via a second joint. In the device
a. A gait generating means for generating a gait of the robot including at least a motion pattern including a target position or posture of at least the foot of the robot and a target pattern of a total floor reaction force acting on the robot;
b. Target foot floor reaction force center point for determining a target foot floor reaction force center point as an action center point on the foot when the total floor reaction force of the generated gait is distributed to each of the foot portions Decision means,
c. An actual floor reaction force detecting means for detecting an actual floor reaction force acting on the foot,
d. Calculating a moment the detected actual floor reaction force about the desired foot floor reaction force central point the determined to a work, foot rotating amount for rotating the foot based on the moment at least the calculated Foot rotation amount determining means for determining
e . A force component of an actual total floor reaction force is obtained from the detected actual floor reaction force to calculate a deviation of the generated gait from a force component of the total floor reaction force, and the foot based on the calculated deviation. Compensation displacement determining means for determining the compensation displacement of the part,
f . The foot portion of the position 置Ma other while rotating the attitude based on the foot rotating amount the determined, by moving the target position or posture of the foot based on the determined compensation displacement, said Foot position / posture correction means for correcting the target position or posture of the foot,
and
g . Joint displacement means for displacing the previous SL first and second joint on the basis of the target position or posture of the foot which has been modified by front Kiashi unit position and orientation correcting means,
A control device for a legged mobile robot, comprising: 少なくとも基体と、前記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、
ａ．前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、
ｂ．前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、
ｃ．前記足部に作用する実床反力を検出する実床反力検出手段、
ｄ．少なくとも前記検出された実床反力に基づいて前記足部を回転させる足部回転量を決定する足部回転量決定手段、
ｅ．前記検出された実床反力から実全床反力の力成分を求めて前記生成された歩容の全床反力の力成分との偏差を算出し、前記算出した偏差に基づいて前記足部の補償変位を決定する補償変位決定手段、
ｆ．前記決定された足部回転量に基づいて前記足部の位置または姿勢を、前記決定された目標足部床反力中心点またはその近傍まわりに回転させつつ、前記決定された補償変位に基づいて前期足部の目標位置または姿勢を移動させることによって、前記足部の目標位置または姿勢を修正する足部位置・姿勢修正手段、
および
ｇ．前記足部位置・姿勢修正手段によって修正された前記足部の目標位置または姿勢に基づいて前記第１および第２の関節を変位させる関節変位手段、
を備えたことを特徴とする脚式移動ロボットの制御装置。Control of a legged mobile robot comprising at least a base body and a plurality of leg portions each having a foot portion connected to the base body via a first joint and connected to the tip thereof via a second joint. In the device
a. A gait generating means for generating a gait of the robot including at least a motion pattern including a target position or posture of at least the foot of the robot and a target pattern of a total floor reaction force acting on the robot;
b. Target foot floor reaction force center point for determining a target foot floor reaction force center point as an action center point on the foot when the total floor reaction force of the generated gait is distributed to each of the foot portions Decision means,
c. An actual floor reaction force detecting means for detecting an actual floor reaction force acting on the foot,
d. Foot rotating amount determining means for determining a foot rotating amount for rotating the foot based on the actual floor reaction force which is at least the detection,
e . A force component of an actual total floor reaction force is obtained from the detected actual floor reaction force to calculate a deviation of the generated gait from a force component of the total floor reaction force, and the foot based on the calculated deviation. Compensation displacement determining means for determining the compensation displacement of the part,
f . The position 置Ma other attitude of the foot based on the foot rotating amount the determined while rotating the also determined desired foot floor reaction force central point is in the vicinity around the determined compensated Foot position / posture correction means for correcting the target position or posture of the foot by moving the target position or posture of the previous foot based on the displacement;
and
g . Joint displacement means for displacing the previous SL first and second joint on the basis of the target position or posture of the foot which has been modified by front Kiashi unit position and orientation correcting means,
A control device for a legged mobile robot, comprising: 前記足部位置・姿勢修正手段は、さらに前記決定された足部回転量に基づいて前記足部の位置または姿勢が、前記決定された目標足部床反力中心点またはその近傍まわりに回転するように、前記足部の目標位置または姿勢を修正することを特徴とする請求項１項記載の脚式移動ロボットの制御装置。Before Kiashi unit position and posture correcting means, further position 置Ma other orientation of said determined based on the foot rotating amount the foot is, also the determined desired foot floor reaction force central point to rotate about the vicinity of the target position 置Ma other control device of a legged mobile robot according to claim 1, wherein said modifying the posture of the foot. さらに、
ｈ．前記ロボットに実際に作用する全床反力モーメント、または前記ロボットに実際に作用する全床反力モーメントから前記足部に作用する床反力モーメントを減算して得たモーメントのいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる足部移動量を決定する足部移動量決定手段、
を備え、前記足部位置・姿勢修正手段は、前記決定された足部回転量、前記決定された補償変位および前記決定された足部移動量に基づいて前記足部の目標位置または姿勢を修正することを特徴とする請求項１項から３項のいずれかに記載の脚式移動ロボットの制御装置。further,
h . Calculate either the total floor reaction force moment actually acting on the robot or the moment obtained by subtracting the floor reaction force moment acting on the foot from the total floor reaction force moment actually acting on the robot. , foot moving amount determining means for determining a foot moving amount for moving the foot in accordance with the moment at least the calculated,
Comprising a front Kiashi unit position and posture correcting means, foot rotating amount the determined was the target position 置Ma of the foot based on the determined compensating displacement and the determined foot moving amount the legged mobile robot control system according to any one of 3 claims 1 to wherein, characterized in that to correct the posture. 前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モーメントを求め、前記足部回転量決定手段または前記足部移動量決定手段は、少なくとも前記検出された実床反力と前記求めた姿勢安定化補償全床反力モーメントに基づいて前記足部回転量または足部移動量を決定することを特徴とする請求項１項から４項のいずれかに記載の脚式移動ロボットの制御装置。The total floor reaction force determined the posture stabilization compensating moment of total floor reaction force to be added to the target pattern of the foot rotating amount determining hands Dhamma others the foot moving amount determining means, the actual floor that is at least the detected the foot rotating Ryoma other on the basis of the obtained posture stabilization compensating moment of total floor reaction force and reaction force from claim 1, wherein, characterized in that determining a foot moving amount to one of the four terms The control apparatus of the legged mobile robot as described. 前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差に基づいて求めることを特徴とする請求項５項に記載の脚式移動ロボットの制御装置。6. The control apparatus for a legged mobile robot according to claim 5, wherein the posture stabilization compensating total floor reaction force moment is obtained based on at least an inclination deviation of the robot. 前記補償変位算出手段は、
ｉ．前記ロボットの基体加速度を検出する基体加速度検出手段、
および
ｊ．前記検出された基体加速度に基づき、前記ロボットを安定化するための補償床反力の力成分を決定する補償全床反力成分決定手段、
を備え、前記決定された補償全床反力の力成分から前記偏差を減算して得た差に基づいて前記足部の補償変位を決定することを特徴とする請求項１項から６項のいずれかに記載の脚式移動ロボットの制御装置。The compensation displacement calculating means includes
i . Substrate acceleration detecting means for detecting substrate acceleration of the robot;
and
j . The detected based on the base body acceleration, determines a compensating floor reaction force of the force component to stabilize the robot compensating total floor reaction force component determination means,
The provided, from the force component of the determined compensating total Yukahan force 6 of claims 1, wherein, characterized in that to determine the compensation displacement of the foot based on a difference obtained by subtracting the deviation The control apparatus of the leg type mobile robot in any one. 前記補償変位決定手段は、前記差に所定の重みを乗じて前記足部の補償変位を足部のそれぞれに分配することを特徴とする請求項７項記載の脚式移動ロボットの制御装置。8. The control apparatus for a legged mobile robot according to claim 7, wherein the compensation displacement determining means distributes the compensation displacement of the foot to each of the foot by multiplying the difference by a predetermined weight. 前記補償変位決定手段は、前記足部が離床しているとき、前記重みを零にすることを特徴とする請求項８項記載の脚式移動ロボットの制御装置。9. The control apparatus for a legged mobile robot according to claim 8, wherein the compensation displacement determining means sets the weight to zero when the foot is getting out of bed. 前記補償変位決定手段は、前記重みを前記実全床反力の力成分の周波数に応じて可変にすることを特徴とする請求項８項記載の脚式移動ロボットの制御装置。9. The control apparatus for a legged mobile robot according to claim 8, wherein the compensation displacement determining means makes the weight variable according to the frequency of the force component of the actual total floor reaction force. 前記補償変位決定手段は、前記足部の補償変位を重力方向について決定することを特徴とする請求項１項から１０項のいずれかに記載の脚式移動ロボットの制御装置。The compensation displacement determination means is a control system of a legged mobile robot according to claim 1 wherein 10 wherein, characterized in that to determine the direction of gravity compensation displacement of the foot. 前記補償変位決定手段は、前記足部の補償変位を重心と目標全床反力中心点を結ぶ線分の方向について決定することを特徴とする請求項１項から１０項のいずれかに記載の脚式移動ロボットの制御装置。The compensation displacement determining means according to any one of 10 claims 1 to wherein, characterized in that to determine the direction of a line connecting the center of gravity and the desired total floor reaction force central point compensation displacement of the foot portion Control device for legged mobile robots. 前記補償全床反力の力成分を零またはその近傍に設定することを特徴とする請求項７項から１２項のいずれかに記載の脚式移動ロボットの制御装置。The control device for a legged mobile robot according to any one of claims 7 to 12, wherein the force component of the compensating total floor reaction force is set to zero or in the vicinity thereof.
JP12523298A 1998-04-20 1998-04-20 Control device for legged mobile robot Expired - Lifetime JP3629142B2 (en)

Priority Applications (5)

Application Number Priority Date Filing Date Title
JP12523298A JP3629142B2 (en) 1998-04-20 1998-04-20 Control device for legged mobile robot
PCT/JP1999/002075 WO1999054095A1 (en) 1998-04-20 1999-04-19 Controller for legged mobile robot
US09/673,629 US6289265B1 (en) 1998-04-20 1999-04-19 Controller for legged mobile robot
DE69943148T DE69943148D1 (en) 1998-04-20 1999-04-19 CONTROL UNIT FOR A LEG ROBOT
EP99913713A EP1120203B1 (en) 1998-04-20 1999-04-19 Controller for legged mobile robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP12523298A JP3629142B2 (en) 1998-04-20 1998-04-20 Control device for legged mobile robot

Publications (2)

Publication Number Publication Date
JPH11300660A JPH11300660A (en) 1999-11-02
JP3629142B2 true JP3629142B2 (en) 2005-03-16

Family

ID=14905094

Family Applications (1)

Application Number Title Priority Date Filing Date
JP12523298A Expired - Lifetime JP3629142B2 (en) 1998-04-20 1998-04-20 Control device for legged mobile robot

Country Status (1)

Country Link
JP (1) JP3629142B2 (en)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3726032B2 (en) 2001-04-27 2005-12-14 本田技研工業株式会社 Target motion generator for legged mobile robot
JP4184679B2 (en) * 2001-08-01 2008-11-19 本田技研工業株式会社 Method for estimating floor reaction force of bipedal mobile body and method for estimating joint moment of bipedal mobile body
JP3674778B2 (en) 2001-09-27 2005-07-20 本田技研工業株式会社 Leg joint assist device for legged mobile robot
JP3665013B2 (en) 2001-11-09 2005-06-29 本田技研工業株式会社 Leg joint assist device for legged mobile robot
EP1475198B1 (en) 2002-01-18 2007-05-23 Honda Giken Kogyo Kabushiki Kaisha Controller of legged mobile robot
KR100425694B1 (en) * 2002-02-06 2004-04-01 엘지전자 주식회사 Walking control method for robot
JP2006088258A (en) 2004-09-22 2006-04-06 Honda Motor Co Ltd Leg joint assisting device of leg type mobile robot
JP5219956B2 (en) * 2009-07-23 2013-06-26 本田技研工業株式会社 Control device for moving body

Also Published As

Publication number Publication date
JPH11300660A (en) 1999-11-02

Similar Documents

Publication Publication Date Title
JP3629133B2 (en) Control device for legged mobile robot
EP1120203B1 (en) Controller for legged mobile robot
JP3634238B2 (en) Floor shape estimation device for legged mobile robot
EP1514777B1 (en) Control system of legged mobile robot
JP3726009B2 (en) Floor shape estimation device for legged mobile robot
US5355064A (en) Control system for legged mobile robot
US5432417A (en) Locomotion control system for legged mobile robot
JP4912891B2 (en) Legged mobile robot and its control program
JPH05337849A (en) Attitude stabilization control device for leg type mobile robot
JPH05305584A (en) Walking control device for leg type mobile robot
JP3629143B2 (en) Control device for legged mobile robot
JP3270766B2 (en) Control device for legged mobile robot
JPH05305579A (en) Walking control device for leg type mobile robot
JPH05305585A (en) Walking control device for leg type mobile robot
JP3629142B2 (en) Control device for legged mobile robot
JPH05305583A (en) Walking control device for leg type mobile robot
JPH05305586A (en) Walking control device for leg type mobile robot
JP4237130B2 (en) Control device for legged mobile robot
JP3726097B2 (en) Posture control device for legged mobile robot
JP3183557B2 (en) Walking control device for legged mobile robot
JPH06182679A (en) Control device for link type robot
JP4946566B2 (en) Walking robot and walking control method

Legal Events

Date Code Title Description
A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20040831

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20041029

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20041124

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20041210

R150 Certificate of patent or registration of utility model

Free format text: JAPANESE INTERMEDIATE CODE: R150

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20071217

Year of fee payment: 3

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20081217

Year of fee payment: 4

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20081217

Year of fee payment: 4

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20091217

Year of fee payment: 5

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20091217

Year of fee payment: 5

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20101217

Year of fee payment: 6

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20101217

Year of fee payment: 6

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20111217

Year of fee payment: 7

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20111217

Year of fee payment: 7

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20121217

Year of fee payment: 8

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20131217

Year of fee payment: 9

EXPY Cancellation because of completion of term