JP3629143B2 - 脚式移動ロボットの制御装置 - Google Patents
脚式移動ロボットの制御装置 Download PDFInfo
- Publication number
- JP3629143B2 JP3629143B2 JP12523398A JP12523398A JP3629143B2 JP 3629143 B2 JP3629143 B2 JP 3629143B2 JP 12523398 A JP12523398 A JP 12523398A JP 12523398 A JP12523398 A JP 12523398A JP 3629143 B2 JP3629143 B2 JP 3629143B2
- 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 - Fee Related
Links
Images
Landscapes
- Control Of Position Or Direction (AREA)
- Manipulator (AREA)
Description
【０００１】
【発明の属する技術分野】
この発明は脚式移動ロボットの制御装置、詳しくはその姿勢制御装置に関し、より詳しくは２足歩行ロボットなどの脚式移動ロボットの脚部の動作をコンプライアンス制御し、脚式移動ロボットに作用する床反力を適切に制御するようにしたものに関する。
【０００２】
【従来の技術】
最も基本的で単純な脚式移動ロボット、より具体的には２足歩行ロボットの制御装置は、目標運動パターン生成装置と関節駆動制御装置から構成される。目標運動パターン生成装置は、少なくとも目標運動パターンを生成する。通常、歩行の運動パターンは、それから動力学的計算によって算出される、即ち、オイラー・ニュートン方程式を解くことによって求められるＺＭＰ軌跡が予め設定しておいた望ましい軌跡になるように生成される。関節駆動制御装置は、歩容生成装置が生成する各関節の変位指令に追従するように各関節を制御する。
【０００３】
ここで、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）は、運動パターンによって発生する慣性力と重力の合力のモーメントが、鉛直軸まわりの成分を除き、０となる床面上の作用点を意味する。
【０００４】
尚、その装置においては、歩容生成装置が平らな床面を想定して歩容を生成していたにも関わらず、図３３に示すように、現在、両脚支持期の初期に、前側の足平が予期しない路面を踏んでしまうと、その足平に想定していた以上の過大な床反力が発生し、ロボットが傾斜する。その問題を解決するために、本出願人は、例えば特開平５−３０５５８６号公報において２足歩行の脚式移動ロボットのその種の制御装置を提案している。
【０００５】
そこにおいては、上体傾斜を検出して上体姿勢を復元させるのに必要な復元モーメント要求量を求めると共に、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメント成分を検出し、それを復元モーメント要求量に一致させようと各足平を上下および回転させるように制御している。この実全床反力モーメントは、各実足平床反力の合力が目標全床反力中心点（目標ＺＭＰ）まわりに発生させるモーメントである。
【０００６】
図３３に示すような予期しなかった傾斜があった場合を例にとって先に提案した制御（以下『両脚コンプライアンス制御』という）を説明する。尚、説明のため、この図に示すように各足平に番号を付す。歩容生成部は平らな床面を想定して歩容を生成していたにも関わらず、図３３に示すように、現在、両脚支持期の初期に第１足平が予期しなかった斜面を踏んだため、第１足平に望ましい値よりも大きな足平床反力が発生した瞬間であると仮定する。また、この瞬間ロボットは未だ望ましい姿勢（上体傾斜０）であったと仮定する。
【０００７】
提案した制御装置では、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントが検出される。この瞬間では、この実全床反力モーメントは、第１足平床反力の鉛直成分が過大であるため、ロボットを後に転倒させる方向に作用する。
【０００８】
このモーメントを０にしようと、図３４に示すごとく、仮想床面Ａ−Ａ’を想定し、各足平をあたかも仮想床面上に乗せたまま、仮想床面を目標全床反力中心点（目標ＺＭＰ）まわりに適当な角度Δθだけ回転させた位置に各足平の位置を移動させる。
【０００９】
それにより、第１足平床反力の鉛直成分が減少すると共に、第２足平床反力の鉛直成分が増大する。この結果、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントがほぼ０になる。即ち、床に予期しなかった斜面があっても、両脚コンプライアンス制御が正常に働くので、ロボットを転倒させないで歩行継続させることができる。
【００１０】
しかしながら、この提案技術だけでは両脚支持期に各足平実床反力を制御することができないので、足平の接地点あたりの床形状に予期しない局所的な傾きや凹凸があると、足平の接地性が低下してスピンしやすくなったり、急激な姿勢変化を起こして転倒する場合がある。
【００１１】
例えば、図３５に示すように、両脚支持期に第１足平のつまさきが予期しない突起（段差）を踏んでしまうと、両脚支持期は、第１足平のつまさきが急激に下がりつつある時期であるので、つまさきで床を強く蹴ってしまい、第１足平床反力の鉛直成分が急増する。その結果、目標全床反力中心点（目標ＺＭＰ）まわりに急激に実全床反力モーメントが発生し、最悪の場合、両脚コンプライアンスによって姿勢を復元させようとしても間に合わずに転倒する。
【００１２】
また、両脚支持期で倒れなかったとしても、その直後に第２足平を床から離したとき、目標全床反力中心点（目標ＺＭＰ）は第１足平のかかとにあるにもかかわらず、かかとが浮いているために実全床反力中心点はつまさきにあるので、目標全床反力中心点（目標ＺＭＰ）まわりにロボットを後に倒そうとする実全床反力モーメントが発生し、転倒する。
【００１３】
即ち、この両脚コンプライアンス制御は、長い距離でゆったりと変化する大域的な傾斜やうねりには対応できるが、足平の着地点の局所的な傾きや段差には対応できないと言える。
【００１４】
上記した両脚コンプライアンス制御とは別に、本出願人は、例えば特開平５−３０５５８４号公報において、２足歩行ロボットの足首部にゴムなどのばね特性を持った着地衝撃吸収機構を備えると共に、各足首まわりの実足平床反力モーメント成分を検出し、それを０にしようと各足首を回転させる足首コンプライアンス制御を提案している。
【００１５】
上記した問題点を解決するため、両脚コンプライアンス制御に加えて、この特開平５−３０５５８４号公報で提案する技術（以下『足首コンプライアンス制御』という）を併用することもできる。
【００１６】
その結果、足首コンプライアンス制御によって、図３６に示すように、予期しなかった第１足平床反力モーメントを打ち消す方向に第１足首を回転させ、かかとも床に接地させることができる。従って、その後の片脚支持期になっても上述のようにロボットを転倒させることはない。
【００１７】
【発明が解決しようとする課題】
しかしながら、上記した両脚コンプライアンス制御および足首コンプライアンス制御を単純に併用するだけでは、２種の制御が干渉しあい、実全床反力と実各足平床反力が望ましい値からずれたり発振してしまう問題があった。
【００１８】
従って、この発明の目的は上記した不都合を解消することにあり、脚式移動ロボットに作用する実床反力を、干渉を生じることなく、容易かつ適切に制御することができる脚式移動ロボットの制御装置を提供することにある。
【００１９】
さらに、ロボットの実各足平床反力の力成分を独立に制御すれば、例えばロボットを進行方向に推進させる力を、接地圧の高い足平には大きくすると共に、低い足平には小さくすることによって、姿勢安定性を向上させてスリップを効果的に防止することができる。また、ロボットの実各足平床反力の力成分を適正に制御することで、一方の足平が他方の足平の発生する進行方向に推進させる力の負荷となっている状態なども解消させることができ、関節アクチュエータの負担を軽減させることができる。
【００２０】
従って、この発明の第２の目的は、ロボットの実各足平床反力の力成分を独立に制御することで、姿勢安定性を向上させてスリップを効果的に防止すると共に、関節アクチュエータの負担を軽減させるようにした脚式移動ロボットの制御装置を提供することにある。
【００２１】
この発明の第３の目的は、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる脚式移動ロボットの制御装置を提供することにある。
【００２２】
この発明の第４の目的は、脚式移動ロボットに作用する床反力をさらに制御することによって、脚式移動ロボットの姿勢安定化制御を容易にすることができる脚式移動ロボットの制御装置を提供することにある。
【００２３】
この発明の第５の目的は、さらに、脚式移動ロボットに作用する床反力を適切に制御することによって、脚式移動ロボットの接地性を高め、歩行時のスリップや前述のスピンを防止することができる脚式移動ロボットの制御装置を提供することにある。
【００２４】
【課題を解決するための手段】
上記の目的を達成するために、請求項１項にあっては、少なくとも基体と、前記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、前記足部に作用する実床反力を検出する実床反力検出手段、前記決定された目標足部床反力中心点まわりに前記検出された実床反力が作用するモーメントを算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる足部回転量を決定する足部回転量決定手段、前記検出された実床反力から実全床反力に影響しない内力成分を求めて前記生成された歩容の内力成分との偏差を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位を決定する補償変位決定手段、前記決定された足部回転量に基づいて前記足部の位置または姿勢を回転させつつ、前記決定された補償変位に基づいて前記足部の目標位置または姿勢を移動させることによって、前記足部の目標位置または姿勢を修正する足部位置・姿勢修正手段、および前記足部位置・姿勢修正手段によって修正された前記足部の目標位置・姿勢に基づいて前記第１および第２の関節を変位させる関節変位手段を備える如く構成した。
【００２５】
請求項２項にあっては、少なくとも基体と、前記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、前記足部に作用する実床反力を検出する実床反力検出手段、少なくとも前記検出された実床反力に基づいて前記足部を回転させる足部回転量を決定する足部回転量決定手段、前記検出された実床反力から実全床反力に影響しない内力成分を求めて前記生成された歩容の内力成分との偏差を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位を決定する補償変位決定手段、前記決定された足部回転量に基づいて前記足部の位置または姿勢を、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転させつつ、前記決定された補償変位に基づいて前期足部の目標位置または姿勢を移動させることによって、前記足部の目標位置または姿勢を修正する第２の足部位置・姿勢修正手段、および前記足部位置・姿勢修正手段によって修正された前記足部の位置または姿勢に基づいて前記第１および第２の関節を変位させる関節変位手段を備える如く構成した。
【００２６】
請求項３項にあっては、前記足部位置・姿勢修正手段は、さらに前記決定された足部回転量に基づいて前記足部の位置または姿勢が、前記決定された目標足部床反力中心点またはその近傍まわりに回転するように、前記足部の目標位置または姿勢を修正する如く構成した。
【００２７】
請求項４項にあっては、さらに、前記ロボットに実際に作用する全床反力モーメント、または前記ロボットに実際に作用する全床反力モーメントから前記足部に作用する床反力モーメントを減算して得たモーメントのいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる足部移動量を決定する足部移動量決定手段を備え、前記足部位置・姿勢修正手段は、前記決定された足部回転量、前記決定された補償変位および前記決定された足部移動量に基づいて前記足部の目標位置または姿勢を修正する如く構成した。
【００２８】
請求項５項にあっては、前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モーメントを求め、前記足部回転量決定手段または前記足部移動量決定手段は、少なくとも前記検出された実床反力と前記求めた姿勢安定化補償全床反力モーメントに基づいて前記足部回転量または足部移動量を決定する如く構成した。
【００２９】
請求項６項にあっては、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差に基づいて求める如く構成した。
【００３０】
請求項７項にあっては、前記補償変位決定手段は、前記偏差に所定の重みを乗じて前記足部の補償変位を足部のそれぞれに分配する如く構成した。
【００３１】
請求項８項にあっては、前記所定の重みが時変特性および周波数特性の少なくともいずれかを持つ如く構成した。
【００３２】
【作用】
請求項１項にあっては、脚式移動ロボットに作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に近い制御を行っても、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。
【００３３】
また、ロボットの実各足平床反力が作り出す内力成分（実全床反力に影響しない成分の組み合わせ）を独立に制御することで姿勢安定性を向上させてスリップを効果的に防止することができると共に、関節アクチュエータの負担を軽減させることができる。
【００３４】
さらには、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。
【００３５】
請求項２項にあっては、請求項１項と同様の作用効果を得ることができる。
【００３６】
請求項３項にあっては、請求項１項と同様の作用効果を得ることができると共に、床反力をより適切に制御することができる。
【００３７】
請求項４項にあっては、請求項１項と同様の作用効果を得ることができると共に、特に姿勢制御に重要な全床反力を一層適切に制御することができる。
【００３８】
請求項５項にあっては、請求項１項と同様の作用効果を得ることができると共に、姿勢安定化能力を向上させることができる。
【００３９】
請求項６項にあっては、前記したと同様の作用効果を得ることができると共に、姿勢安定化能力を一層向上させることができる。
【００４０】
請求項７項にあっては、前記したと同様の作用効果を得ることができる。
【００４１】
請求項８項にあっては、前記したと同様の作用効果を得ることができると共に、補償変位の分配を一層適正に行うことができる。
【００４２】
【発明の実施の形態】
以下、添付図面を参照してこの発明に係る脚式移動ロボットの制御装置を説明する。尚、脚式移動ロボットとしては２足歩行ロボットを例にとる。
【００４３】
図１はその脚式移動ロボットの制御装置を全体的に示す概略図である。
【００４４】
図示の如く、２足歩行ロボット１は左右それぞれの脚部リンク２に６個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。６個の関節は上から順に、股（腰部）の脚部回旋用の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、股（腰部）のロール方向（Ｘ軸まわり）の関節１４Ｒ，１４Ｌ、股（腰部）のピッチ方向（Ｙ軸まわり）の関節１２Ｒ，１２Ｌ、膝部のピッチ方向の関節１６Ｒ，１６Ｌ、足首のピッチ方向の関節１８Ｒ，１８Ｌ、同ロール方向の関節２０Ｒ，２０Ｌから構成される。
【００４５】
関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には足平（足部）２２Ｒ，２２Ｌが取着されると共に、最上位には上体（基体）２４が設けられ、その内部にマイクロコンピュータからなる制御ユニット２６（後述）などが格納される。上記において股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から、足関節（足首関節）は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２８Ｒ，２８Ｌ、膝関節と足関節とは下腿リンク３０Ｒ，３０Ｌで連結される。
【００４６】
上記の構成により、脚部リンク２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６＊２＝１２個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行させることができる（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）。
【００４７】
尚、この明細書で後述する上体の位置およびその速度は、上体２４の所定位置、具体的には上体２４の重心位置などの代表点の位置およびその移動速度を意味する。
【００４８】
図１に示す如く、足関節の下方には公知の６軸力センサ４４が取着され、力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚとを測定し、足部の着地の有無および床反力（接地荷重）などを検出する。
【００４９】
また、上体２４には傾斜センサ６０が設置され、Ｚ軸（鉛直方向（重力方向））に対する傾きとその角速度を検出する。傾斜センサ６０はＧセンサおよび角速度センサとから構成され、Ｇセンサは上体２４のＸ，Ｙ，Ｚ軸方向の加速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコーダが設けられる。
【００５０】
図２に示すように、足平２２Ｒ（Ｌ）の上方には、ばね機構３２が装備されると共に、足底にはゴムなどからなる足底弾性体３４が貼られる。ばね機構３２は具体的には、足平２２Ｒ（Ｌ）に取り付けられた方形状のガイド部材と、足首関節１８Ｒ（Ｌ）および６軸力センサ４４側に取り付けられ、前記ガイド部材に弾性材を介して微動自在に収納されるピストン状部材とからなる。
【００５１】
図中に実線で表示された足平２２Ｒ（Ｌ）は、床反力を受けていないときの状態を示す。床反力を受けるとバネ機構３２と足底弾性体３４がたわみ、足平は図中に点線で表示された位置・姿勢に移る。この構造は、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。尚、その詳細は前記した特開平５−３０５５８４号に記載されているので、詳細な説明は省略する。
【００５２】
更に、図１では図示を省略するが、２足歩行ロボット１の適宜な位置にはジョイスティック６２が設けられ、外部から必要に応じて直進歩行しているロボットを旋回させるなど歩容に対する要求を入力できるように構成される。
【００５３】
図３は制御ユニット２６の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ６０などの出力はＡ／Ｄ変換器７０でデジタル値に変換され、その出力はバス７２を介してＲＡＭ７４に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ７６を介してＲＡＭ７４内に入力される。
【００５４】
制御ユニット内にはＣＰＵからなる第１、第２の演算装置８０，８２が設けられており、第１の演算装置８０は後述の如く、ＲＯＭ８４に格納されている歩容に基づいて後述の如く関節角変位指令を算出し、ＲＡＭ７４に送出する。また第２の演算装置８２はＲＡＭ７４からその指令と検出された実測値とを読み出し、各関節の駆動に必要な制御値を算出してＤ／Ａ変換器８６とサーボアンプを介して各関節を駆動する電動モータに出力する。
【００５５】
ここで、この明細書および図面で使用する用語について定義する（尚、定義しない用語に関しては、本出願人が前記した技術とは別に提案した出願（特願平８−２１４２６１号）で使用した定義に従う）。
【００５６】
『歩容』は、ロボット工学における一般的な定義と異なり、目標運動パターンと床反力パターンを合わせたものを指称する意味で使用する。但し、床反力パターンとしては、例えば『ＺＭＰ軌跡だけ』というように、部分情報であっても良い。そのため、目標運動パターンだけを出力して床反力パターンに関する情報を出力しない装置に対して「歩容生成装置」と言う言葉を用いない。
【００５７】
各脚には、通し番号をつける。第ｎ脚に作用する床反力を第ｎ足平床反力という（ｎ：１または２。以下同じ）。全脚に作用する床反力を合成したものを全床反力という（ロボット工学では一般的には床反力と呼ばれるが、足平床反力と区別するためにここでは『全床反力』という）。
【００５８】
足平床反力は作用点とそこにかかる力と力のモーメントによって表現され、同一の足平床反力に対して、表現の組み合わせは無限通りある。その中には、鉛直軸まわりの成分を除くモーメント成分が０でかつ作用点が床面上にある表記が存在する。この表現における作用点を、ここでは足平床反力中心点という（本出願人が別途提案した後述する特開平６−７９６５７号では『接地圧重心点』と称した）。
【００５９】
同様に、全床反力は作用点とそこにかかる力と力のモーメントによって表現され、同一の全床反力に対して表現の組み合わせは無限通りある。その中には、鉛直軸まわりの成分を除くモーメント成分が０でかつ作用点が床面上にある表現が存在する。この表現における作用点を、ここでは全床反力中心点という。
【００６０】
全床反力の目標値を目標全床反力という。目標全床反力は、通常、目標運動パターンに対して動力学的に平衡する全床反力である。従って、通常、目標全床反力中心点は、目標ＺＭＰに一致する。
【００６１】
尚、始めに触れたように、目標ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ ）は次のように定義される。即ち、目標運動パターンによって発生する慣性力と重力の合力を動力学的に求め、これが床面上のある点に作用するモーメントが、鉛直軸まわりの成分を除き０であるならば、その点を目標ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）という。目標ＺＭＰは、合力の垂直方向力成分が０でない限り、一義的に求められる。以下の説明では、理解しやすくするために、目標ＺＭＰという言葉を用いる場合もあるが、厳密には目標全床反力中心点と言うべき箇所が多い。
【００６２】
各足平床反力の目標値を目標各足平床反力という。但し、目標全床反力とは異なり、目標運動パターンが決まっていても目標各足平床反力は一義的には決定されない。実際のロボットに作用する全床反力を実全床反力という。実際のロボットに作用する各足平床反力を実各足平床反力という。
【００６３】
ここで、この発明の課題について再説すると、この発明は、局所的な傾きや段差に対して先に提案した両脚コンプライアンス制御では良好な姿勢安定性を得ることが困難であると共に、その不都合は足首コンプライアンス制御を用いれば解消することができるが、両者を単純に併用するだけでは干渉し合い、実全床反力と実各足平床反力が望ましい値からずれたり、発振する不都合があった。
【００６４】
その問題点を先の図３３に示す状況で説明すると、第１足平はかかとに予期しなかった過大な床反力を受けているため、第１足首のまわりに過大な実足平床反力モーメントが発生する。足首コンプライアンス制御は、このモーメントを０にしようと第１足首を図３６に示すように回転させる。
【００６５】
しかしながら、足首の回転によって、第１足平のかかと位置が高くなるので、第１足平床反力の鉛直成分が減少する。この結果、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントが変化する。これは、両脚コンプライアンス制御の制御量である実全床反力モーメントが、足首コンプライアンス制御に干渉されることを意味する。
【００６６】
従って、足首コンプライアンス制御による干渉を考慮しないで、両脚コンプライアンス制御を、足首コンプライアンス制御がない場合と同様に働かせると、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントが０からずれたり、干渉による振動や発振が生じる。
【００６７】
それを防止する方法のひとつとして、両脚コンプライアンス制御と足首コンプライアンス制御の間の干渉量を求め、それを打ち消すような操作量を加えることによって干渉しないようにすることが考えられるが、歩行中は姿勢が時々刻々と変化し、干渉関係も時々刻々と変化するため、この手法で干渉を回避することは極めて難しい。
【００６８】
また、図３６に示す状況では、第１足平が接触している床は想定していた床よりも登り傾斜なので、第１足平は、目標歩容よりもつまさきを上げるべきである。それにもかかわらず、足首コンプライアンス制御によりつまさきが下がってしまうことは、足首コンプライアンス制御が適切に作用していないとも言える。
【００６９】
以上のように、足首コンプライアンス制御は足平の着地点の局所的な床の傾きや段差には効果があるが、長い距離でゆったりと変化する傾斜やうねりには、却って悪影響を与える場合がある。
【００７０】
従って、実施の形態に係る装置においては、脚式移動ロボットに作用する床反力、より具体的には、目標全床反力中心点まわりの実全床反力モーメントと、目標各足平床反力中心点まわりの実各足平床反力モーメントを容易かつ適切に制御できるようにした。
【００７１】
また、それによって大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに安定した姿勢でロボットを歩行継続させるようにした。
【００７２】
尚、上記した課題を解決すべく、本出願人は先に脚式移動ロボットの制御装置を提案しており、この発明の目的はその提案技術をさらに改良し、実各足平床反力の力成分を独立に制御して姿勢安定性を向上させてスピンなどを防止すると共に、関節アクチュエータの負担も軽減するようにした脚式移動ロボットの制御装置を提供することにある。
【００７３】
図４は、この実施の形態に係る脚式移動ロボットの制御装置（主として図３の第１の演算装置８０に相当）の構成および動作を機能的に示すブロック図である。以下、図４を参照してこの装置の全体構成を概説する。
【００７４】
この装置は歩容生成器を備え、歩容生成器は目標歩容を生成し、出力する。目標歩容は、前述の定義の通り、目標運動パターンと目標床反力パターン、より具体的には目標上***置・姿勢軌道、目標足平位置・姿勢軌道、目標全床反力中心点（目標ＺＭＰ）軌道および目標全床反力軌道からなる。目標床反力パターンは、このように、目標全床反力中心点軌跡を含む（後述する機構変形補償を行わないならば、目標床反力パターンとしては目標全床反力中心点軌跡だけでも良い）。
【００７５】
この実施の形態において歩容生成器が出力する目標全床反力は、目標運動パターンに対して動力学的に平衡する全床反力である。従って、目標全床反力中心点は、目標ＺＭＰに一致する。
【００７６】
図５にロボット１が平地を歩行するときの目標運動パターンの一例を示す。これに対応する目標ＺＭＰ軌道の床面上軌跡を図６に、タイム・チャートを図７に示す。この歩容の期間に床に接触したままの足平を、第１足平、もう一方を第２足平ということとする。尚、歩容生成器の詳細は先に提案した特願平８−２１４２６１号に詳細に述べられているので、これ以上の説明は省略する。
【００７７】
図４の説明に戻ると、この装置は目標床反力分配器を備え、目標床反力分配器は、目標全床反力中心点（目標ＺＭＰ）と目標足平位置・姿勢を主な入力とし、目標各足平床反力中心点を決定して出力する。実際には、歩容生成器から歩容のパラメータ（例えば、両脚支持期の時間や遊脚足平の目標着地位置など）や、歩容の時期・時刻（例えば、現在時刻が両脚支持期の初めから０．１ｓｅｃであるなど）などの情報も必要に応じて取り込む。
【００７８】
図５に示すような歩容に対して、目標床反力分配器は、目標各足平床反力中心点が以下の条件を満足するように設定する。
条件１）目標各足平床反力中心点軌跡は連続である。
条件２）両脚支持期では、目標第１足平床反力中心点はかかとに、目標第２足平床反力中心点はつまさきに存在する。
条件３）このとき目標第１足平床反力中心点と目標第２足平床反力中心点を結ぶ線分上に、目標全床反力中心点が存在する。
条件４）片脚支持期では、目標第１足平床反力中心点は、目標全床反力中心点に一致する。
条件５）片脚支持期の間に、目標第２足平床反力中心点は、つまさきからかかとに移動する。
【００７９】
これら条件を満足する目標第１足平床反力中心点軌跡のタイム・チャートを図８に、目標第２足平床反力中心点軌跡のタイム・チャートを図９に示す。尚、この図では足首（関節１８，２０Ｒ（Ｌ））から足平２２Ｒ（Ｌ）への垂直投影点を原点とし、図１に示すように足平前方向をＸ軸の正の向き、足平左方向をＹ軸の正の向きにとる。
【００８０】
目標床反力分配器は、更に、付随的ではあるが、目標各足平床反力も決定して出力する。目標各足平床反力は、ばね機構３２などのたわみ補償のために必要である。
【００８１】
次式を用いて上記のように設定された目標各足平床反力中心点に対応する目標各床反力を決定すれば、目標各足平床反力の合力は目標全床反力に一致しなければならないと言う条件を満足する。
【００８２】
【００８３】
このように求めた目標各足平床反力は連続的に変化するので、衝撃の少ない歩行を実現するために適している。尚、上記の詳細は本出願人が別途提案した技術（特開平６−７９６５７号）に記述されている。
【００８４】
図４の説明に戻ると、この装置は姿勢安定化制御演算部を備え、姿勢安定化制御演算部はロボットのセンサ情報に基づいてロボットの状態を推定し、補償全床反力を算出する。即ち、実際にロボットが歩行あるいは直立しているときなどには後述する変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたとしても、ロボットの位置・姿勢は必ずしも望ましい位置・姿勢にならない。
【００８５】
ロボットの姿勢を長期的に安定化させるためには、ロボットを望ましい位置・姿勢に復元させるために必要な力とモーメントを求め、これを目標全床反力中心点（目標ＺＭＰ）を作用点として付加的に発生させる必要がある。この付加的な力とモーメントを補償全床反力という。また、補償全床反力のモーメント成分を補償全床反力モーメントという。
【００８６】
尚、脚式移動ロボットの目標歩容が床反力以外の反力を環境から受けるように想定し、それを例えば、目標対象物反力と称し、先に述べた目標ＺＭＰの定義を次のように拡張しても良い。即ち、目標運動パターンによって発生する慣性力と重力と目標対象物反力の合力を動力学的に求め、それが床面上のある点に作用するモーメントが、鉛直軸まわりの成分を除いて零であるならば、その点を改めて目標ＺＭＰとするようにしても良い。
【００８７】
もし、ロボット１が完全剛体であって、変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたと仮定すると、足平のばね機構３２および足底弾性体３４のたわみによって生じるロボット全体の位置・姿勢の摂動的な運動は、以下の６自由度に分解できる。
【００８８】
モード１）目標全床反力中心点（目標ＺＭＰ）を中心とした前後軸まわり回転（即ち、左右傾き）
モード２）目標全床反力中心点（目標ＺＭＰ）を中心とした左右軸まわり回転（即ち、前後傾き）
モード３）目標全床反力中心点（目標ＺＭＰ）を中心とした鉛直軸まわり回転（即ち、スピン）
モード４）前後平行移動揺れ
モード５）左右平行移動揺れ
モード６）上下平行移動揺れ
【００８９】
この内で、モード４とモード５は、足平のばね機構３２および弾性体３４が前後左右方向のせん断力を受けて撓むことによって発生するものである。ばね機構３２および足底弾性体３４は剪断方向の剛性が高いように製作するので、この揺れは極めて少なく、歩行に及ぼす悪影響はほとんどない。
【００９０】
先に提案した技術では主としてモード１とモード２を扱っていたが、この発明に係る実施の形態では、それに加えて、ロボット全体の挙動（モード１から６）に直接的には影響しない内力の制御を行う。ここでいう「内力」とは、全床反力に影響を与えない各足平床反力の成分の組み合わせを意味する。尚、理解の便宜のため、重複するが、先の提案技術を説明する。
【００９１】
モード１を制御するための操作量は、補償全床反力の前後軸（Ｘ軸）まわりモーメント成分である。モード２を制御するための操作量は、補償全床反力の左右軸（Ｙ軸）まわりモーメント成分である。従って、補償全床反力の成分の内、前後軸方向モーメント成分と左右軸方向モーメント成分だけを求めれば良い。他の成分は、この実施の形態では用いないので０で良い。
【００９２】
尚、以降は次の定義に従う。即ち、補償全床反力のモーメント成分を補償全床反力モーメントＭｄｍｄ（詳しくは目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントＭｄｍｄ）という。図５に示す如く、ロボットの前方向をＸ軸、左横方向をＹ軸、上方向をＺ軸にとり、第１足平の足首直下の床面上の点を原点とした座標系を支持脚座標系と呼び、断らない限り、位置、力およびモーメントはこの座標系で表現されるものとする。また、ＭｄｍｄのＸ成分をＭｄｍｄｘ、Ｙ成分をＭｄｍｄｙ、Ｚ成分をＭｄｍｄｚと記述する。上体２４の傾斜偏差（即ち、実上体傾斜−目標上体傾斜）θｅｒｒ のＸ成分をθｅｒｒｘ， Ｙ成分をθｅｒｒｙ、これらの時間微分値を（ｄθｅｒｒｘ ／ ｄｔ），（ｄθｅｒｒｙ ／ ｄｔ）と記述する。
【００９３】
ＭｄｍｄｘおよびＭｄｍｄｙは、例えば次式の制御則によって決定される。
ここで、Ｋｔｈｘ，Ｋｔｈｙ，ＫｗｘおよびＫｗｙは、上体傾斜安定化制御ゲインである。
【００９４】
後述する複合コンプライアンス動作決定部は、目標全床反力と補償全床反力の合力に実全床反力を一致させようと働く。
【００９５】
図４の説明に戻ると、この装置は実各足平床反力検出器を備え、実各足平床反力検出器は、６軸力センサ４４によって実各足平床反力（その合力が実全床反力）を検出する。更に、関節のエンコーダによって検出される実変位（あるいは変位指令）に基づき、上体に固定された座標系に対する各足平の相対位置・姿勢を算出し、それによって６軸力センサ４４の検出値を座標変換し、上体に固定された座標系で表現された実各足平床反力を算出した後、更に、支持脚座標系に変換する。
【００９６】
この装置はロボット幾何学モデル（逆キネマティクス演算部）を備え、ロボット幾何学モデルは、上***置・姿勢と足平位置・姿勢を入力されると、それらを満足する各関節変位を算出する。この実施の形態におけるロボット１のような１脚あたりの関節自由度が６である場合には、各関節変位は一義的に求まる。
【００９７】
この実施の形態では逆キネマティクスの解の式を直接的に求めておき、式に上***置・姿勢と足平位置・姿勢を代入するだけで各関節変位を得るようにした。即ち、ロボット幾何学モデルは、目標上***置・姿勢と複合コンプライアンス動作決定部で修正された修正目標足平位置・姿勢軌道（機構変形補償入り修正目標足平位置・姿勢軌道）を入力し、それらから１２個の関節（１０Ｒ（Ｌ）など）の関節変位指令（値）を算出する。尚、ヤコビアンによって関節角変位を得ても良い。
【００９８】
この装置は変位コントローラ（前記した第２の演算装置８２に同じ）を備え、変位コントローラは、ロボット幾何学モデル（逆キネマティクス演算部）で算出された関節変位指令（値）を目標値としてロボット１の１２個の関節の変位を追従制御する。
【００９９】
前記した複合コンプライアンス動作決定部は、以下の２つの要求を満足させようと、目標足平位置・姿勢軌道を修正する。
【０１００】
要求１）ロボットの位置・姿勢制御のために、実全床反力を姿勢安定化制御部が出力する補償全床反力（モーメントＭｄｍｄ）と目標全床反力の合力に追従させる。ロボットの姿勢傾きだけを制御したい場合には、目標全床反力中心点まわりの実全床反力水平方向モーメント成分だけを補償全床反力モーメントＭｄｍｄに追従させる。
【０１０１】
要求２）各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。
【０１０２】
尚、補足すると、通常は実全床反力を補償全床反力と目標全床反力の合力に一致させながら目標各足平床反力中心点まわりの実各足平床反力モーメントを０にすることが、物理的に不可能な場合が多い。従って、要求１）と要求２）は完全に両立させることはできず、ある点で妥協しなくてはならない。
【０１０３】
上記を前提として図１０フロー・チャート（構造化フロー・チャート）を参照してこの装置の動作を説明する。尚、図の左端に該当する処理を行う図４装置の構成要素を示す。
【０１０４】
先ずＳ１０において装置を初期化し、Ｓ１２を経てＳ１４に進み、タイマ割り込みを待機する。タイマ割り込みは５０ｍｓごとになされ、即ち、制御周期は５０ｍｓである
【０１０５】
続いてＳ１６に進んで歩容の切り替わり目、即ち、支持脚の切り替わり目か否か判断し、否定されるときはＳ２２に進むと共に、肯定されるときはＳ１８に進んでタイマｔをイニシャライズし、Ｓ２０に進んで目標歩容パラメータを設定する。前記の如く、歩容パラメータは、運動パラメータと床反力パラメータ（ＺＭＰ軌道パラメータ）から構成される。
【０１０６】
続いてＳ２２に進み、目標歩容の瞬時値を決定する。ここで『瞬時値』は制御周期ごとの値を意味し、目標歩容瞬時値は、目標上***置・姿勢、目標各足平位置・姿勢、および目標ＺＭＰ位置から構成される。尚、ここで『姿勢』はＸ，Ｙ，Ｚ空間における『向き』を意味する。
【０１０７】
続いてＳ２４に進んで目標各足平床反力中心点を求める。これは、目標床反力分配器の説明で述べたように行う。具体的には、図８および図９に示すように設定した目標各足平床反力中心点軌跡の現在時刻ｔにおける値を求めることで行う。
【０１０８】
続いてＳ２６に進んで目標各足平床反力を求める。これは目標床反力分配器の説明で述べた式１を用いて目標各足平床反力を演算することで行う。
【０１０９】
続いてＳ２８に進み、前記した傾斜センサ６０などの出力から上体２４の傾斜などロボット１の状態を検出する。
【０１１０】
続いてＳ３０に進み、ロボット１の状態などから姿勢を安定化するための（目標全床反力中心点（目標ＺＭＰ）まわりの）補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを求める。具体的には、上体傾斜が検出されたとき姿勢安定化を図るために前記した式２に従って補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを演算する。
【０１１１】
続いてＳ３２に進んで実各足平床反力を検出する。これは前記の如く、６軸力センサ４４の出力から検出する。
【０１１２】
続いてＳ３４に進み、両脚補償角θｄｂｖおよび各足平補償角θｎｘ（ｙ）を決定する。これは、前記した複合コンプライアンス動作決定部が行う作業である。
【０１１３】
その複合コンプライアンス動作決定部の作業について説明する。説明の便宜のため、両脚支持期において図１１に示すように第１足平２２Ｒ（Ｌ）と第２足平２２Ｌ（Ｒ）に実各足平床反力が作用している状況と仮定する。
【０１１４】
ここでベクトルＦｎａｃｔは第ｎ足平床反力の力成分を表す。ベクトルＭｎａｃｔは第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎａｃｔの向きは、向きに対して時計回りのモーメントが床から足平に作用していることを表す。
【０１１５】
この瞬間の目標全床反力は、図１２に示すようになっていると仮定する。ちなみに、目標全床反力中心点（目標ＺＭＰ）における目標全床反力モーメントベクトルＭｓｕｍｒｅｆは垂直である（定義により、目標ＺＭＰは目標全床反力モーメントの水平方向成分が０である点であるから）。
【０１１６】
これを式１に従って目標各足平床反力に分配すると、図１３に示すようになる。同図において、ベクトルＦｎｒｅｆは目標第ｎ足平床反力の力成分を表す。ベクトルＭｎｒｅｆは目標第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎｒｅｆの向きの表現は、Ｍｎａｃｔと同様である。
【０１１７】
説明のため、上体姿勢が左後ろに倒れそうな状態を想定する。
【０１１８】
前述の姿勢安定化制御演算部では、ロボット１の上体傾斜偏差検出値θｅｒｒｘ，θｅｒｒｙに基づいて補償全床反力モーメントＭｄｍｄを算出する。この実施の形態では鉛直軸（Ｚ軸）まわりのスピンを制御しないので、補償全床反力モーメントＭｄｍｄの鉛直軸成分は０である。上***置の揺れも制御しないので、補償全床反力モーメントの力成分も０である。この状態に対応する補償全床反力モーメントＭｄｍｄを図１４に示す。
【０１１９】
姿勢を復元させるためには、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平成分を、目標全床反力モーメントＭｓｕｍｒｅｆと補償全床反力モーメントＭｄｍｄの和の水平成分に追従させれば良い。
【０１２０】
一方、目標全床反力中心点（目標ＺＭＰ）では目標全床反力モーメントＭｓｕｍｒｅｆの水平方向成分は０である。従って、前後左右の姿勢傾きを復元させるためには、目標ＺＭＰまわりの実全床反力モーメントの水平成分を、Ｍｄｍｄの水平成分に追従させれば良い。
【０１２１】
この実施の形態にあっては複合コンプライアンス動作決定部は、以下の要求をできる限り満足するように足平の位置・姿勢を修正する。
要求１）ロボットの姿勢傾斜を安定化制御するために、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平方向（Ｘ，Ｙ軸方向）成分を、補償全床反力モーメントＭｄｍｄの水平方向成分に追従させる。
要求２）各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。
【０１２２】
但し、前述の通り、要求１）と要求２）は、完全に両立させることはできず、ある点で妥協しなくてはならない。
【０１２３】
足平の位置・姿勢の修正は、この実施の形態では次のように行う。
１）目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を含み、かつ水平面と垂直な平面の法線ベクトルＶを求める。Ｖの大きさは１とする。Ｖを図１５に示す。
【０１２４】
２）目標第１足平床反力中心点Ｑ１の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに、ある回転角θｄｂｖだけ回転移動する。移動した後の点をＱ１’とする。同様に、目標第２足平床反力中心点Ｑ２の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに回転角θｄｂｖだけ回転移動する。移動した後の点をＱ２’とする。
【０１２５】
この回転角θｄｂｖを両脚補償角という。始点がＱ１、終点がＱ１’のベクトルをベクトルＱ１Ｑ１’とする。同様に、始点がＱ２、終点がＱ２’のベクトルをベクトルＱ２Ｑ２’とする。図１６にＱ１’とＱ２’を示す。
【０１２６】
３）目標第１足平を、姿勢は変えずにベクトルＱ１Ｑ１’だけ平行移動（ほぼ上下移動）させる。同様に、目標第２足平を、姿勢は変えずにベクトルＱ２Ｑ２’だけ平行移動させる。移動後の目標各足平を図１６に太線で示す。
【０１２７】
４）次に、目標第１足平をＱ１’を中心に、前後方向軸（Ｘ軸）まわりに回転角θ１ｘ、左右方向軸（Ｙ軸）まわりに回転角θ１ｙだけ回転させる。同様に、目標第２足平を目標第２足平をＱ２’を中心に前後方向軸（Ｘ軸）まわりに回転角θ２ｘ、左右方向軸（Ｙ軸）まわりに回転角θ２ｙだけ回転させる。回転角θｎｘ，θｎｙをそれぞれ第ｎ足平Ｘ補償角、第ｎ足平Ｙ補償角という。回転後の目標各足平を図１７に太線で示す。
【０１２８】
以上の補償動作量が過大でなければ、接地圧力分布は変わっても、接地領域（足底面の圧力が正の領域）は変わらない。このような場合には、補償動作量に比例して各足平に装着されたばね機構３２や足底弾性体３４などが変形し、変形量に応じた実各足平床反力が発生する。この結果、補償動作量と補償動作によって発生する実床反力の変化量との間の関係は、以下に示す良好な特性を持つ。
【０１２９】
特性１）両脚補償角θｄｂｖだけを操作して目標各足平位置を移動させると、下がった足平の実足平床反力の力成分が増加し、上がった足平の実足平床反力の力成分が減少する。このとき、修正目標各足平床反力中心点まわりの実各足平床反力モーメントは、ほとんど変化しない。
【０１３０】
特性２）第ｎ足平Ｘ補償角だけを操作して目標第ｎ足平姿勢を回転させると、目標第ｎ足平床反力中心点に作用する実第ｎ足平床反力のモーメントのＸ成分だけが変化し、その他の床反力成分は少ししか変化しない。同様に、第ｎ足平Ｙ補償角だけを操作して目標第ｎ足平姿勢を回転させると、実第ｎ足平床反力のモーメントのＹ成分だけが変化し、その他の床反力成分は少ししか変化しない。
【０１３１】
特性３）両脚補償角θｄｂｖ、各足平Ｘ補償角および各足平Ｙ補償角を同時に操作すると、実各足平床反力の変化量は、それぞれを単独に操作したときの変化量の和になる。
【０１３２】
特性１および特性２は、これらの操作に独立性があることを示し、特性３はこれらの操作に線形性があることを示していると言える。
【０１３３】
図１８は複合コンプライアンス動作決定部の演算処理を示すブロック図であり、同図を参照してこの作業を説明する。
【０１３４】
概説すると、補償全床反力モーメント分配器において補償全床反力モーメントＭｄｍｄの分配を行う。次に、実各足平床反力と分配された補償全床反力モーメントなどから、両脚補償角決定部および第ｎ足平Ｘ（Ｙ）補償角決定部において前述の補償角θｄｂｖおよびθｎｘ（ｙ）を決定する。
【０１３５】
次に、決定された各種補償角に基づいて修正目標足平位置・姿勢算出部は、補償された足平位置・姿勢（これを修正目標足平位置・姿勢という）を幾何学演算によって求める。最後に、機構変形補償入り修正目標足平位置・姿勢算出部は、目標各足平床反力によって発生が予想されるばね機構３２や足底弾性体３４の変形量を求め、それらを打ち消すように修正目標足平位置・姿勢をさらに修正する。
【０１３６】
以下詳説すると、補償全床反力モーメント分配器は、補償全床反力モーメントＭｄｍｄを、両脚補償モーメントＭｄｍｄｄｂ、各足平補償モーメントＭｄｍｄ１ｘ，ｙ，Ｍｄｍｄ２ｘ，ｙに分配する。両脚補償モーメントＭｄｍｄｄｂは、両脚補償角（足平上下量）θｄｂｖを操作することによって目標全床反力中心点（目標ＺＭＰ）まわりに各足平床反力の力成分が作るモーメントの目標値である。
【０１３７】
両脚補償モーメントＭｄｍｄｄｂのＶ方向まわりの成分をＭｄｍｄｄｂｖと記述する。尚、ベクトルＶは複合コンプライアンス動作決定部の説明で定義したベクトルである。Ｖに直交し、鉛直方向にも直交するベクトルをＵとすると、両脚補償モーメントＭｄｍｄｄｂのＵ方向成分Ｍｄｍｄｄｂｕは０に設定される。両脚補償角θｄｂｖを操作しても、床反力のＵ方向モーメント成分を発生することはできないからである。
【０１３８】
この実施の形態では補償全床反力モーメントＭｄｍｄの鉛直方向成分が０なので、Ｍｄｍｄｄｂの鉛直方向成分Ｍｄｍｄｄｂｚも０に設定される。
【０１３９】
第１足平補償モーメントＭｄｍｄ１は、第１足平補償角θ１ｘ，θ１ｙを操作することによって目標第１足平床反力中心点まわりに発生させたいモーメントである。第１足平補償モーメントＭｄｍｄ１のＸ成分をＭｄｍｄ１ｘ、Ｙ成分をＭｄｍｄｌｙと記述する。第２足平補償モーメントＭｄｍｄ２は、第２足平補償角θ２ｘ，θ２ｙを操作することによって目標第２足平床反力中心点まわりに発生させたいモーメントである。第２足平補償モーメントＭｄｍｄ２のＸ成分をＭｄｍｄ２ｘ、Ｙ成分をＭｄｍｄ２ｙと記述する。
【０１４０】
分配は、例えば次のように行う。
【０１４１】
ここで、Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔは分配用重み変数である。ＶｘはベクトルＶのＸ成分の値、ＶｙはベクトルＶのＹ成分の値である。この中で、Ｗｉｎｔは、両脚補償角を操作することによって発生した全床反力モーメントを各足平補償角を操作することによって打ち消すためのものである。
【０１４２】
式３と式４の演算処理を行う補償全床反力モーメント分配器のブロック図を図１９に示す。
【０１４３】
歩行時の分配用重み変数Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔの設定例を図２０に示す。図２０のパターンは、以下の注意点を考慮して決定することが望ましい。
【０１４４】
注意点１）両脚補償角と各足平補償角が不連続的に変化すると、関節に過大なトルクが発生する。そこで、両脚補償角と各足平補償角を連続的に変化させるために、分配用重み変数は連続的に変化させる。
【０１４５】
注意点２）両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントが、なるべく補償全床反力モーメントＭｄｍｄに近い値になるように、分配用重み変数を決定する。
【０１４６】
この際、直立時や歩行時など状況に応じて以下に示すように設定方針を変えた方が良い。直立時などのように、両脚補償モーメントのＶ方向成分Ｍｄｍｄｄｂｖ、各足平補償モーメントＭｄｍｄ１，Ｍｄｍｄ２を忠実に実各足平床反力に発生させることができる状況では以下のように設定する。
【０１４７】
この状況では目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平方向成分を、補償全床反力モーメントＭｄｍｄの水平方向成分に一致させるために、（即ち、前述の複合コンプライアンス動作決定部に対する要求１を満足するために、）式５と式６の両方をなるべく満足するように重みを設定すべきである。
【０１４８】
【０１４９】
これに式３、式４を代入すると、式５は式７に、式６は式８に変換される。
【０１５０】
ＭｄｍｄｘとＭｄｍｄｙが任意の値を取っても、式７と式８が恒等的に成立するためには、式９、式１０、および式１１を同時に満足すれば良い。
即ち、以上の状況では式９、式１０および式１１を同時に満足するように、重みを決定すれば良い。
【０１５１】
歩行時ではＭｄｍｄｄｂｖを目標にして両脚補償角θｄｂｖを操作して足平の位置を修正しても、実全床反力モーメントの発生量がＭｄｍｄｄｂｖに較べて不足する場合がある。例えば図２１のように両脚支持期の初期にロボットが後傾して第１足平が未だ着地していない状況では、θｄｂｖによって第１足平の位置を下げても、実床反力は変化しない。
【０１５２】
同様に、Ｍｄｍｄ２を目標にして第２足平補償角θ２を操作して第２足平の角度を修正しても、実床反力モーメントの増加量がＭｄｍｄ２に較べて不足する場合がある。例えば、図２２のように両脚支持期の後半にロボットが後傾している状況では、θ２によって第２足平のかかとを下げても実床反力は変化しない。
【０１５３】
従って、式５、式６を満足するように各重みを設定しても、複合コンプライアンス制御によって発生する実全床反力の増加量が補償全床反力モーメントＭｄｍｄに届かない場合がある。このようなことが生じる可能性が高い状況では、式５、式６の左辺の値を右辺で割った値を１より大きくすべきである。
【０１５４】
歩行時の分配用重み変数設定例である図２０では、Ｗｉｎｔを０に設定することによって、図２１の状況のように、両脚補償角θｄｂｖ を操作しても実全床反力モーメントが発生できなくなっても、各足平補償角を操作して不足分を補うようにした。
【０１５５】
好都合なことに、図２１のように後傾すると第２足平のかかとが結果的に下がって床に接地しやすくなるので、第２足平補償角を操作することによって実全床反力モーメントを発生させることができるようになる。
【０１５６】
また、後傾していないときには両脚補償角θｄｂｖを操作することによる実全床反力モーメントが発生するが、第２足平のかかとが床に接地しないので、第２足平補償角を操作しても実全床反力モーメントは発生しない。
【０１５７】
つまり、両脚補償角θｄｂｖが有効に働くときには各足平補償角が有効に働かず、各足平補償角が有効に働くときには両脚補償角θｄｂｖが有効に働かないので、結果的に両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントの総量は、ほぼ補償全床反力モーメントＭｄｍｄに等しくなる。
【０１５８】
状況によっては、両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントの総量が補償全床反力モーメントＭｄｍｄよりも大きくなってしまう場合がある。
【０１５９】
しかし、この場合でも、Ｍｄｍｄがこの実施の形態のように姿勢安定化のためのフィードバック操作量であるならば、あまり問題にならない。何故ならば、Ｍｄｍｄの大きさが多少違っていても、一般的に制御系に言えることであるが、制御系のオープンループゲインが多少変化するだけで、クローズドループ特性はほとんど変わらないからである。
【０１６０】
注意点３）片脚支持期では、両脚補償角用の分配用重み変数であるＷｄｂｘ，Ｗｄｂｙの絶対値を小さくする。片脚支持期では両脚補償角を変化させても、接地していない足平が無駄に上下するだけで、実各足平床反力は変化しないからである。
【０１６１】
注意点４）足平の接地性を確保するために、目標足平床反力の力成分が小さいときには、その足平の足平補償角のための分配用重み変数の絶対値を小さくする。特に、足平が床から遠く離れているときには、その足平の足平補償角を動かしても、その足平の実足平床反力は変化しないので、不要な動きをさせないためにも、その足平の足平補償角のための分配用重み変数の絶対値を小さくすべきである。
【０１６２】
注意点５）両脚補償角を操作することによって制御できる実全床反力モーメントの方向と、各足平補償角を操作することによって制御できる実全床反力モーメントの方向は通常異なる。
【０１６３】
例えば、両脚補償角θｄｂｖを操作することによって発生する実全床反力モーメントの向きは必ずＶ方向であり、Ｖ方向に直交する成分を発生させることはできない。一方、各足平補償角を操作することによって発生できる実全床反力モーメントの向きは、足平の接地状況によって制約を受ける。
【０１６４】
例えば、つまさきのエッジだけまたはかかとのエッジだけが接地している場合には、エッジ線方向にモーメントを発生することはできない。両脚支持期では、この特性を考慮して、なるべく無駄なく両脚補償角および各足平補償角を操作する。
【０１６５】
例えば、両脚補償角を操作するための分配重みＷｄｂｘ，Ｗｄｂｙは次のように決定する。
【０１６６】
Ｘ成分がＷｄｂｘ、Ｙ成分がＷｄｂｙ、Ｚ成分が０のベクトルをＷｄｂとすると、式３はベクトルＷｄｂとＭｄｍｄの内積になっている。従って、ＭｄｍｄをベクトルＷｄｂ方向成分とその直交成分に分解し、ベクトルＷｄｂ方向成分だけを抽出して、ベクトルＷｄｂの大きさを乗じたものが、式３によって求められるＭｄｍｄｄｂｖであると言える。
【０１６７】
この場合のＭｄｍｄｄｂｖを図２３に示す。これは、両脚補償角を操作することによって実全床反力モーメントのＷｄｂ方向成分を制御するフィードバック制御系を構成することを意味する。もし、Ｗｄｂ方向がベクトルＶと直交していたら、両脚補償角をいくら操作しても実全床反力モーメントのＷｄｂ方向成分は発生しないから、このフィードバック制御系はただ無駄に両脚補償角を操作するだけになる。
【０１６８】
従って、無駄な動きを減らした場合には、Ｗｄｂ方向をベクトルＶ方向に一致させるか、またはなるべく近づけるべきである。また、補償全床反力モーメントＭｄｍｄのＷｄｂ方向成分を、各足平補償角に頼らずに両脚補償角を操作するだけで発生させたいならば、ＷｄｂとＶの内積が１になるように設定する。一部を各足平補償角に頼らせたいならば、ＷｄｂとＶの内積が１より小さくなるように設定する。
【０１６９】
ところで、足平の横幅が狭い場合には、各足平補償角を操作することによって発生し得る実各足平床反力モーメントのＸ成分は小さくなる。この場合には、Ｗｄｂｘを大きめに設定する。Ｗｄｂ方向とベクトルＶ方向は一致しなくなり、両脚補償角の変動が増加するが、安定性が増す。
【０１７０】
両脚補償角決定部について更に詳説すると、図２４は両脚補償角決定部の演算処理のブロック図であり、両脚補償角θｄｂｖは図示の如く演算される。
【０１７１】
図２４を参照して説明すると、目標第１足平床反力中心点Ｑ１に作用するＦ１ａｃｔと目標第２足平床反力中心点Ｑ２に作用するＦ２ａｃｔが、目標全床反力中心点Ｐのまわりに発生させるモーメントＭｆ１ｆ２ａｃｔを、次式により求める。
【０１７２】
ここで、ＰＱ１は始点がＰ、終点がＱ１のベクトル、ＰＱ２は始点がＰ、終点がＱ２のベクトルである。
【０１７３】
また、式１２の代わりに、次式を用いても実際上はほとんど問題がない。
式１２ａは、目標全床反力中心点まわりに作用する実全床反力モーメントＭａｃｔを算出する式になっている。尚、式１２は、目標全床反力中心点まわりに作用する実全床反力モーメントから、目標各足平床反力中心点まわりに作用する実各足平床反力モーメントを減じたものになっている。請求項４項の記載は、これに基づく。
【０１７４】
次に、Ｍｆ１ｆ２ａｃｔのベクトルＶ方向成分Ｍｆ１ｆ２ａｃｔｖを抽出する。これは、ベクトルの内積演算を用いた次式によって得られる。尚、ベクトルＶは前述の動作説明において図１５に示したＶである。
【０１７５】
次に、Ｍｆ１ｆ２ａｃｔｖをローパスフィルタに通してＭｆ１ｆ２ａｃｔｖｆｉｌｔを得る。
【０１７６】
次に、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを補償用フィルタに通し、それを、Ｍｆ１ｆ２ａｃｔｖｆｉｌｔから減じ、偏差モーメントＶ方向成分Ｍｄｉｆｆｖを得る。
【０１７７】
尚、補償用フィルタは、Ｍｄｍｄｄｂｖから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。
【０１７８】
次に、足平ばね機構などの変形による両脚補償モーメントＶ方向成分への影響を打ち消すための両脚機構変形補償角θｆｆｄｂｖを求める。これは、いわゆるフィードフォワード補償である。
【０１７９】
具体的には、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖと変形量との関係を表す機構コンプライアンスモデルを用い、目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を結ぶ線分の変形角度を求め、それの極性を反転したものを両脚機構変形補償角θｆｆｄｂｖとすれば良い。
【０１８０】
両脚機構変形補償角θｆｆｄｂｖは、近似的には次式により求めれば良い。
ここでαは所定の定数である。
【０１８１】
最後に次式によって両脚補償角θｄｂｖを得る。ここでＫｄｂは制御ゲインであり、通常、これは正の値に設定する。
【０１８２】
第ｎ足平補償角決定部について説明すると、図２５はその中の第１足平Ｘ補償角決定部の演算処理を示すブロック図であり、第１足平Ｘ補償角θ１ｘは図示の如く演算する。説明は省略するが、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙも同様に求める。ここでは第１足平Ｘ補償角θ１ｘを求めるアルゴリズムだけを説明する。
【０１８３】
第１足平床反力モーメントＸ成分Ｍ１ａｃｔｘをローパスフィルタに通してＭ１ａｃｔｆｉｌｔｘを得る。第１足平補償モーメントＸ成分Ｍｄｍｄ１ｘを補償用フィルタに通し、それを、Ｍ１ａｃｔｆｉｌｔｘから減じ、偏差モーメントＭｄｉｆｆ１ｘを得る。両脚補償角決定と同様、補償用フィルタは、Ｍｄｍｄ１ｘから実全床反力までの伝達関数の周波数応答特性を改善するものである。
【０１８４】
次に、両脚補償角決定と同様、足平ばね機構などの変形による第１足平補償モーメントＸ成分への影響を打ち消すための第１足平Ｘ機構変形補償角θｆｆ１ｘを求める。これは、いわゆるフィードフォワード補償である。
【０１８５】
具体的には、第１足平補償モーメントＸ方向成分Ｍｄｍｄ１ｘと変形量との関係を表す機構コンプライアンスモデルを用い、第１足平の変形角度を求め、それの極性を反転したものを第１足平Ｘ機構変形補償角θｆｆ１ｘとすれば良い。
【０１８６】
第１足平Ｘ機構変形補償角θｆｆ１ｘは、近似的には次式により求めれば良い。
ここでα１ｘは所定の定数である。
【０１８７】
最後に次式によって第１足平Ｘ補償角θ１ｘを得る。ここでＫ１ｘは制御ゲインであり、通常、これも正の値に設定する。
尚、図示のブロック線図は、演算処理順序を変えるなどの等価変形をしても良い。
【０１８８】
図１８に戻って説明を続けると、修正目標足平位置・姿勢算出部は、両脚補償角θｄｂｖ、第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙおよび内力補償変位（後述）に基づき、前述の複合コンプライアンス動作を拡張した足平位置・姿勢修正手法に従って目標足平位置・姿勢を修正し、修正目標足平位置・姿勢を得る。
【０１８９】
機構変形量算出部は、目標各足平床反力によって発生が予想されるばね機構３２や足底弾性体３４の変形量を求める。
【０１９０】
機構変形補償入り修正目標足平位置・姿勢算出部は、算出された機構変形量を打ち消すように、修正目標足平位置・姿勢をさらに修正し、機構変形補償入り修正目標足平位置・姿勢を得る。
【０１９１】
例えば、図２６に示すような機構変形量が予想されるときには、機構変形補償入り修正目標足平位置・姿勢は、図２７に実線で示す位置・姿勢に修正される。即ち、図２７に示す機構変形補償後の足平が目標足平床反力を受けて変形したときの位置・姿勢が、図２６に示す床反力を受けないときの機構変形補償前の足平位置・姿勢に一致するように、機構変形補償入り修正目標足平位置・姿勢を算出する。
【０１９２】
機構変形補償は、ばね機構３２や足底弾性体３４の変形によって生じる実足平位置・姿勢のずれをフィードフォワード的に打ち消す制御であり、この制御がない場合に比較し、より一層、目標歩容に近い歩行を実現することができる。
【０１９３】
上記を前提として図１０フロー・チャートの説明に戻ると、前記の如く、Ｓ３４において上記した補償角を決定する。
【０１９４】
図２８はその作業を示すサブルーチン・フロー・チャートである。
【０１９５】
同図を参照して説明すると、先ずＳ１００において前記したベクトルＶを求め、Ｓ１０２に進んで分配用重み変数を図２０に示すように設定し、現在時刻ｔでのこれらの値を求める。続いてＳ１０４に進み、式３および式４によって補償全床反力モーメントＭｄｍｄを両脚補償モーメントＭｄｍｄｄｂｖと各足平補償モーメントＭｄｍｄｎｘ（ｙ）に分配し、Ｓ１０６に進んで既述の如く両脚補償角θｄｂｖを求め、Ｓ１０８に進んで各足平補償角θｎｘ（ｙ）を求める。
【０１９６】
図１０フロー・チャートの説明に戻ると、次いでＳ３５に進んで各足平内力補償変位を決定するが、これについては後述する。
【０１９７】
次いでＳ３６に進み、目標各足平床反力に基づいて機構変形補償量を算出し、Ｓ３８に進んで目標足平位置・姿勢を補償角θｄｂｖ，θｎｘ（ｙ）、および内力補償変位（後述）に応じて修正し、更にこれを機構変形補償量に応じて修正し、機構変形補償入り修正目標足平位置・姿勢を得る。
【０１９８】
次いでＳ４０に進み、上***置・姿勢と機構変形補償入り修正足平位置・姿勢から関節変位指令（値）を算出し、Ｓ４２に進んで実関節変位を算出された関節変位指令（値）にサーボ制御し、Ｓ４４に進んで時刻をΔｔ更新し、Ｓ１４に戻って上記の処理を繰り返す。
【０１９９】
ここで、図１０フロー・チャートのＳ３５の処理について説明する。
【０２００】
上記したＳ３５を除く処理で行われる制御によってロボット全体の挙動に影響を与える実全床反力のモーメント成分と実各足平モーメントを独立に制御することができる。しかし、実各足平床反力の力成分を独立に制御することはできない。
【０２０１】
本出願人がこの出願と同日付けで提案する、実全床反力の力成分のコンプライアンス制御では、実各足平床反力の力成分の和を制御することができるので、実各足平の力成分を独立に制御するためには、実各足平床反力の力成分の差を制御するものを加えれば良い。Ｓ３５の処理はそれを意図するもので、以下の追加される制御を「内力コンプライアンス制御」という。
【０２０２】
この制御は、目標第１足平と目標第２足平の間隔を僅かに延ばしたり、縮めたりすることによって、両足平を結ぶ線分方向に、足平床反力の力成分を、それぞれの足平に互いに逆向きに発生させる。この動作を「逆位相平行移動動作」という。
【０２０３】
この制御を行うと、実全床反力と実各足平モーメントに対してＳ３５の処理を除く処理で述べた制御に干渉することなく、実第１足平の力成分と実第２足平の力成分の比率を変えることができるので、この比率を状況に応じて適切に制御することによって、前記したように、例えばロボットを進行方向に推進させる力を、接地圧の高い足平には大きくすると共に、低い足平には小さくすることによって、姿勢安定性を向上させてスリップを効果的に防止することができる。
【０２０４】
また、一方の足平が他方の足平の発生する進行方向に推進させる力の負荷となっている状態なども解消させることができ、関節アクチュエータの負担を軽減させることができる。
【０２０５】
その意図から、この実施の形態においては、先に提案した装置に、図１８の下部に示す如く、内力補償変位算出部を追加した。具体的には、内力補償変位算出部および内力補償変位を含む修正目標足平位置姿勢算出部は、Ｓ３５の処理を除く処理で得られた修正目標各足平位置・姿勢に以下の操作を加える。
【０２０６】
図２９は内力補償変位算出部の処理を詳細に示すブロック図であり、図３０はその逆位相平行移動動作の説明図である。図２９に示す如く、内力補償変位算出部は、実各足平床反力の力成分と目標各足平床反力の力成分から、図３０に示す各足平内力補償変位Ｄｎ（ｎ＝１，２）を算出する。また、修正目標足平位置姿勢算出部は、その各足平内力補償変位Ｄｎを含め、幾何学演算によって修正足平位置・姿勢を算出する。
【０２０７】
図３０を参照して概説すると、先ず、始点がＱ１’、始点がＱ２’のベクトルと、向きが同じで大きさが１の方向ベクトルを求める。これを以降Ｖ’と記述する。
【０２０８】
次に、Ｓ３５の処理を除く処理で得られた修正目標第１足平を、姿勢を変えずに、Ｄ１Ｖ’だけ平行移動させる。図示の如く、Ｄ１は修正目標第１足平のＶ’方向への移動量であり、以降、第１足平内力補償変位という。Ｄ１は、スカラ変数である。
【０２０９】
同時に、修正目標第２足平を、姿勢を変えずに、Ｄ２Ｖ’だけ平行移動させる。Ｄ２は修正目標第２足平のＶ’方向への移動量であり、以降、第２足平内力補償変位という。Ｄ１も、スカラ変数である。尚、図示の状態では、Ｄ１は負、Ｄ２は正である。通常、Ｄ１とＤ２は極性が逆であるため、両足平は、逆位相に動く。以上が、逆位相平行移動動作である。
【０２１０】
上記を図３１フロー・チャートを参照してさらに詳細に説明する。
【０２１１】
Ｓ２００において図３２の時変特性を示す、予め設定された分配用重み変数Ｗｄ１，Ｗｄ２の現在時刻ｔにおける値を求める。
【０２１２】
続いてＳ２０２に進み、実各足平床反力の力成分および目標各足平床反力の力成分から式１９によって偏差Ｆｄｅｒｒを算出する。即ち、図２９に示すように、実第１足平床反力の力成分Ｆ１ａｃｔ、実第２足平床反力の力成分Ｆ２ａｃｔ、目標第１足平床反力の力成分Ｆ１ｒｅｆ、および目標第２足平床反力の力成分Ｆ２ｒｅｆから、次の式（式１９）によって偏差Ｆｄｅｒｒを算出する。
【０２１３】
Ｆｄｅｒｒ＝Ｆ２ａｃｔ−Ｆ１ａｃｔ−（Ｆ２ｒｅｆ−Ｆ１ｒｅｆ）・・・式１９
尚、目標第１足平床反力の力成分Ｆ１ｒｅｆおよび目標第２足平床反力の力成分Ｆ２ｒｅｆは、前記した目標歩容の中の値である。
【０２１４】
続いてＳ２０４に進み、算出した偏差Ｆｄｅｒｒを制御系の発振を防止するためのローパスフィルタに通し、さらに、ゲインＫｄ倍して差Ｄｉｆｆを算出する。
【０２１５】
続いてＳ２０６に進み、次式（式２０，２１）によって、第１足平内力補償変位Ｄ１と第２足平内力補償変位Ｄ２を算出する。
Ｄ１＝−Ｗｄ１＊Ｄｄｉｆｆ ．．．式２０
Ｄ２＝ Ｗｄ２＊Ｄｄｉｆｆ ．．．式２１
【０２１６】
ここで、Ｗｄ１およびＷｄ２は、非負の分配重み変数である。Ｗｄ１とＷｄ２の和は１のとき、Ｄｄｉｆｆは、修正目標第２足平と修正目標第１足平の間の距離の伸び量を意味する。
【０２１７】
分配用重み変数Ｗｄｎの設定例は図３２に示す通りであるが、図示の例では以下の点を考慮して設定される。即ち、各足平内力補償変位が不連続に変化すると、関節に過大なトルクが発生することから、Ｗｄｎは、連続的に変化するように設定される。
【０２１８】
また、足平が着地する頃にＷｄｎが零でないと、足平着地位置が目標位置からずれ、却ってこの内力制御の支障となる場合がある。従って、足平が着地する付近ではＷｄｎは零にする。また、片足支持期は制御不要であることから、Ｗｄｎを零にすると共に、両脚支持期および直立時にはＷｄ１とＷｄ２の和を１にする。
【０２１９】
かく得られた同位相補償変位Ｄｎも含め、図１０フロー・チャートのＳ３８において機構変形補償入り修正目標足平位置姿勢が算出される。
【０２２０】
具体的には、両脚補償角θｄｂｖ、第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙ、第１足平内力補償変位Ｄ１、および第２足平内力補償変位Ｄ２に基づき、前記した同位相平行移動動作を含む複合コンプライアンス動作に従って修正目標足平位置姿勢が算出される。
【０２２１】
この実施の形態では上記の如く、少なくとも基体（上体２４）と、前記基体に第１の関節（１０，１２，１４Ｒ（Ｌ））を介して連結されると共に、その先端に第２の関節（１８，２０Ｒ（Ｌ））を介して連結される足部（足平２２Ｒ（Ｌ））を備えた複数本（２本）の脚部（脚部リンク２）からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上***置・姿勢、目標足平位置・姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（＝目標ＺＭＰ））を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（歩容生成器、Ｓ１０からＳ２２）、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、Ｓ２４，Ｓ２６）、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（６軸力センサ４４、実各足平床反力検出器、Ｓ３２）、前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモーメント（実第ｎ足平床反力モーメントＭａｃｔｘ，ｙ，ｚ）を算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる回転量（両脚補償角θｄｂｖ，ｚ、第ｎ足平補償角θｎｘ，ｙ，ｚ）を決定する足部回転量決定手段（複合コンプライアンス動作決定部、Ｓ３２からＳ３４、両脚補償角決定部、第ｎ足平補償角決定部、Ｓ１００からＳ１０８）、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が回転するように前記目標位置および／または姿勢を修正する第１の足部位置・姿勢修正手段（複合コンプライアンス動作決定部、Ｓ３８，Ｓ４０、修正目標足平位置・姿勢算出部）、前記検出された実床反力から実全床反力に影響しない内力成分（Ｆ１ａｃｔ＋Ｆ２ａｃｔ）を求めて前記生成された歩容の内力成分（Ｆ１ｒｅｆ＋Ｆ２ｒｅｆ）との偏差Ｆｄｅｒｒ，Ｄｄｉｆｆを算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位Ｄｎを算出する補償変位算出手段（内力補償変位算出部、Ｓ３５，Ｓ２００からＳ２０６）、前記算出された補償変位に基づいて目標足部位置・姿勢を修正する第２の足部位置・姿勢修正手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３８）および前記第１および前記第１および第２の足部位置・姿勢手段によって修正された足部の位置・姿勢に基づいて前記ロボットの第１および第２の関節（１０，１２，１４，１８，２０Ｒ（Ｌ））を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、Ｓ４０，Ｓ４２）を備える如く構成した。
【０２２２】
また、少なくとも基体（上体２４）と、前記基体に第１の関節（１０，１２，１４Ｒ（Ｌ））を介して連結されると共に、その先端に第２の関節（１８，２０Ｒ（Ｌ））を介して連結される足部（足平２２Ｒ（Ｌ））を備えた複数本（２本）の脚部（脚部リンク２）からなる脚式移動ロボット１の制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上***置・姿勢、目標足平位置・姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（＝目標ＺＭＰ））を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（歩容生成器、Ｓ１０からＳ２２）、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、Ｓ２４）、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（６軸力センサ４４、実各足平床反力検出器、Ｓ３２）、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量（両脚補償角θｄｂｖ，ｚ、第ｎ足平補償角θｎｘ，ｙ，ｚ）を決定する足部回転量決定手段（複合コンプライアンス動作決定部、Ｓ３２，Ｓ３４、両脚補償角決定部、第ｎ足平補償角決定部、Ｓ１００からＳ１０８）、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および／または姿勢を修正する第１の足部位置・姿勢修正手段（複合コンプライアンス動作決定部、Ｓ３８，Ｓ４０、修正目標足平位置・姿勢算出部）、前記検出された実床反力から実全床反力に影響しない内力成分（Ｆ１ａｃｔ＋Ｆ２ａｃｔ）を求めて前記生成された歩容の内力成分（Ｆｒｅｆ１＋Ｆｒｅｆ２）との偏差Ｆｄｅｒｒ，Ｄｄｉｆｆを算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位Ｄｎを算出する補償変位算出手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３５，Ｓ２００からＳ２０６）、前記算出された補償変位に基づいて目標足部位置・姿勢を修正する第２の足部位置・姿勢修正手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３８）および前記第１および前記第１および第２の足部位置・姿勢手段によって修正された足部の位置・姿勢に基づいて前記ロボットの第１および第２の関節（１０，１２，１４，１８，２０Ｒ（Ｌ））を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、Ｓ４２）を備えるように構成した。
【０２２３】
また、前記第１の足部位置・姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および／または姿勢を修正する如く構成した。
【０２２４】
さらに、前記ロボットに実際に作用する全床反力モーメント（より正確にはモーメント成分ＰＱ１ ＊Ｆ１ａｃｔ ＋ＰＱ２ ＊Ｆ２ａｃｔ ＋Ｍ１ａｃｔ ＋Ｍ２ａｃｔ）、または前記ロボットに実際に作用する全床反力のモーメント（ＰＱ１ ＊Ｆ１ａｃｔ ＋ＰＱ２ ＊Ｆ２ａｃｔ ＋Ｍ１ａｃｔ ＋Ｍ２ａｃｔ ）から前記足部に作用する床反力モーメント（Ｍ１ａｃｔ ＋Ｍ２ａｃｔ）を減算して得たモーメント（Ｍｆ１ｆ２ａｃｔ＝ＰＱ１ ＊Ｆ１ａｃｔ ＋ＰＱ２ ＊Ｆ２ａｃｔ ）のいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる移動量（θｄｂｖ，ｚ）を決定する足部移動量決定手段（複合コンプライアンス動作決定部、Ｓ３４、両脚補償角決定部、Ｓ１００からＳ１０８）を備え、前記第１の足部位置・姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置および／または姿勢を修正するように構成した。
【０２２５】
また、前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モーメント（補償全床反力Ｍｄｍｄ）を求め、前記足部回転量決定手段および／または前記足部移動量決定手段は、少なくとも前記検出された実床反力（実各足平床反力）と前記求めた姿勢安定化補償全床反力モーメントに基づいて前記足部の回転量および／または移動量を決定する（Ｓ３４，Ｓ１００からＳ１０８）如く構成した。
【０２２６】
また、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差（θｅｒｒｘ，ｙ）に基づいて求める（Ｓ２８，Ｓ３０）如く構成した。
【０２２７】
また、前記補償変位算出手段は、前記偏差（Ｆｄｅｒｒ，Ｄｄｉｆｆ）に所定の重み（Ｗｄｎ）を乗じて前記足部の補償変位を足部のそれぞれに分配する如く構成した。
【０２２８】
また、前記所定の重みが時変特性および／または周波数特性を持つ如く構成した。
【０２２９】
上記の如く構成したので、これによって、概括すれば、実全床反力の制御と実各足平床反力の制御が殆ど干渉しないようになり、それらを容易に制御することができる。
【０２３０】
即ち、この実施の形態に係る装置は、先に提案した技術に対して以下の点を改良した。即ち、特開平５−３０５５８４号公報で提案した足首コンプライアンス制御では、足首または足底の基準点などの足平に固定された点における実床反力モーメントを検出し、それに基づいて前記固定された点を中心に足平を回転させていたが、この実施の形態に係る装置では、移動する目標足平床反力中心点における実各足平床反力モーメントを算出し、それに基づいて目標足平床反力中心点を中心に足平を回転させるように変更し、その点まわりのモーメントを望ましい値に制御するようにした。
【０２３１】
この結果、実全床反力と実各足平床反力がほとんど干渉することなく、容易に制御することが可能となった。より干渉を少なくするために、各瞬間における想定していた足底接地領域内にもっと適切な点を選定しても良い。
【０２３２】
従って、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。
【０２３３】
また、ロボットの実各足平床反力の力成分を独立に制御することで姿勢安定性を向上させてスリップを効果的に防止することができると共に、関節アクチュエータの負担を軽減させることができる。
【０２３４】
また、上記した実施の形態において、ばね機構３２（および足底弾性体３４）自身はこの発明の本質部分ではない。この発明の本質はフィードバック制御部分にあり、機構変形補償は付随的なものである。
【０２３５】
また、上記した実施の形態において、ブロック線図は演算処理順序を変えるなどの等価変形をしても良い。
【０２３６】
また、上記した実施の形態を、先に提案した出願の第１の実施の形態に関して説明したが、第２あるいはそれ以降の実施の形態に関して追加しても良い。
【０２３７】
また、この発明を２足歩行ロボットに関して説明してきたが、２足歩行ロボットに限らず、多脚ロボットにも応用することができる。
【０２３８】
【発明の効果】
請求項１項にあっては、脚式移動ロボットに作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に近い制御を行っても、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。
【０２３９】
また、ロボットの実各足平床反力が作り出す内力成分（実全床反力に影響しない成分の組み合わせ）を独立に制御することで姿勢安定性を向上させてスリップを効果的に防止することができると共に、関節アクチュエータの負担を軽減させることができる。
【０２４０】
さらには、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。
【０２４１】
請求項２項にあっては、請求項１項と同様の作用効果を得ることができる。
【０２４２】
請求項３項にあっては、請求項１項と同様の作用効果を得ることができると共に、床反力をより適切に制御することができる。
【０２４３】
請求項４項にあっては、請求項１項と同様の作用効果を得ることができると共に、特に姿勢制御に重要な全床反力を一層適切に制御することができる。
【０２４４】
請求項５項にあっては、請求項１項と同様の作用効果を得ることができると共に、姿勢安定化能力を向上させることができる。
【０２４５】
請求項６項にあっては、前記したと同様の作用効果を得ることができると共に、姿勢安定化能力を一層向上させることができる。
【０２４６】
請求項７項にあっては、前記したと同様の作用効果を得ることができる。
【０２４７】
請求項８項にあっては、前記したと同様の作用効果を得ることができると共に、補償変位の分配を一層適正に行うことができる。
【図面の簡単な説明】
【図１】この発明に係る脚式移動ロボットの制御装置を全体的に示す説明図である。
【図２】図１に示す２足歩行ロボットの足部の構造を示す説明側面図である。
【図３】図１に示す２足歩行ロボットの制御ユニットの詳細を示すブロック図である。
【図４】この発明に係る脚式移動ロボットの制御装置の構成および動作を機能的に示すブロック図である。
【図５】図１に示す脚式移動ロボットが平地を歩行するときの運動パターンの一例を示す説明図である。
【図６】図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡の床面上軌跡を示す説明図である。
【図７】図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡のタイム・チャートである。
【図８】図５の運動パターンに対応する所定の条件を満たすように設定した目標第１足平床反力中心点軌跡のタイム・チャートである。
【図９】図５の運動パターンに対応する所定の条件を満たすように設定した目標第２足平床反力中心点軌跡のタイム・チャートである。
【図１０】図４と同様に、この発明に係る脚式移動ロボットの制御装置の動作を示すフロー・チャートである。
【図１１】図１０フロー・チャートの内の両脚補償角などの演算処理を行う、図４に示す複合コンプライアンス動作決定部の動作を説明するための、両脚支持期に第１足平と第２足平に実各足平床反力が作用している状況を示す説明図である。
【図１２】図１１に示す状況における目標全床反力の設定を示す説明図である。
【図１３】図１１に示す状況における目標各足平床反力の分配を示す説明図である。
【図１４】図１１に示す状況における補償全床反力モーメントを示す説明図である。
【図１５】図１１に示す状況における、各足平床反力中心点を含み、水平面に垂直な平面の法線ベクトルＶを示す説明図である。
【図１６】図１１に示す状況における、目標各足平床反力中心点を目標全床反力中心点（目標ＺＭＰ）まわりに、所定角度θｄｂｖだけ回転させたときの状態を示す説明図である。
【図１７】図１６に示す状況における、各足平を前後方向軸および左右方向軸まわりに所定角度θｎｘ，θｎｙだけ回転させたときの状態を示す説明図である。
【図１８】図４の複合コンプライアンス動作決定部の演算処理を示すブロック図である。
【図１９】図１８に示す補償全床反力モーメント分配器の演算処理を示すブロック図である。
【図２０】図１８に示す補償全床反力モーメント分配器の、両脚補償角などを操作するための分配重み変数の設定例を示すタイム・チャートである。
【図２１】図２０の補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。
【図２２】図２１と同様に、補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。
【図２３】両脚補償角を操作するための分配重みを所定の条件で決定したときの両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを示す説明図である。
【図２４】図１８に示す両脚補償角決定部の演算処理を示すブロック図である。
【図２５】図１８に示す各足平の補償角決定部の演算処理を示すブロック図である。
【図２６】図１８に示す機構変形補償入り修正目標足平位置・姿勢算出部の演算処理を説明するための説明図である。
【図２７】図２６と同様に、図１８に示す機構変形補償入り修正目標足平位置・姿勢算出部の演算処理を説明するための説明図である。
【図２８】図１０フロー・チャートの内の両脚補償角などの決定作業を示すサブルーチン・フロー・チャートである。
【図２９】図１８の内力補償変位算出部の処理を詳細に示すブロック図である。
【図３０】図２９ブロック図の処理を説明する、図１６と同様な説明図である。
【図３１】図１８の内力補償変位算出部の処理を示すフロー・チャートである。
【図３２】図２９および図３１で使用する重みの設定例を示す説明図である。
【図３３】２足歩行ロボットが予期しなかった傾斜面を歩行するときの説明図である。
【図３４】図３３に示す２足歩行ロボットに対して先に提案した両脚コンプライアンス制御を行った場合の説明図である。
【図３５】図３３に類似する、２足歩行ロボットが予期しなかった突起を踏んだときの説明図である。
【図３６】図３５に示す状況で、先に提案した足首コンプライアンス制御を行ったときの説明図である。
【符号の説明】
１ ２足歩行ロボット（脚式移動ロボット）
２ 脚部リンク
１０，１２，１４Ｒ，Ｌ 股関節
１６Ｒ，Ｌ 膝関節
１８，２０Ｒ，Ｌ 足関節
２２Ｒ，Ｌ 足平（足部）
２４ 上体
２６ 制御ユニット
３２ ばね機構
３４ 足底弾性体
４４ ６軸力センサ
６０ 傾斜センサ
【発明の属する技術分野】
この発明は脚式移動ロボットの制御装置、詳しくはその姿勢制御装置に関し、より詳しくは２足歩行ロボットなどの脚式移動ロボットの脚部の動作をコンプライアンス制御し、脚式移動ロボットに作用する床反力を適切に制御するようにしたものに関する。
【０００２】
【従来の技術】
最も基本的で単純な脚式移動ロボット、より具体的には２足歩行ロボットの制御装置は、目標運動パターン生成装置と関節駆動制御装置から構成される。目標運動パターン生成装置は、少なくとも目標運動パターンを生成する。通常、歩行の運動パターンは、それから動力学的計算によって算出される、即ち、オイラー・ニュートン方程式を解くことによって求められるＺＭＰ軌跡が予め設定しておいた望ましい軌跡になるように生成される。関節駆動制御装置は、歩容生成装置が生成する各関節の変位指令に追従するように各関節を制御する。
【０００３】
ここで、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）は、運動パターンによって発生する慣性力と重力の合力のモーメントが、鉛直軸まわりの成分を除き、０となる床面上の作用点を意味する。
【０００４】
尚、その装置においては、歩容生成装置が平らな床面を想定して歩容を生成していたにも関わらず、図３３に示すように、現在、両脚支持期の初期に、前側の足平が予期しない路面を踏んでしまうと、その足平に想定していた以上の過大な床反力が発生し、ロボットが傾斜する。その問題を解決するために、本出願人は、例えば特開平５−３０５５８６号公報において２足歩行の脚式移動ロボットのその種の制御装置を提案している。
【０００５】
そこにおいては、上体傾斜を検出して上体姿勢を復元させるのに必要な復元モーメント要求量を求めると共に、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメント成分を検出し、それを復元モーメント要求量に一致させようと各足平を上下および回転させるように制御している。この実全床反力モーメントは、各実足平床反力の合力が目標全床反力中心点（目標ＺＭＰ）まわりに発生させるモーメントである。
【０００６】
図３３に示すような予期しなかった傾斜があった場合を例にとって先に提案した制御（以下『両脚コンプライアンス制御』という）を説明する。尚、説明のため、この図に示すように各足平に番号を付す。歩容生成部は平らな床面を想定して歩容を生成していたにも関わらず、図３３に示すように、現在、両脚支持期の初期に第１足平が予期しなかった斜面を踏んだため、第１足平に望ましい値よりも大きな足平床反力が発生した瞬間であると仮定する。また、この瞬間ロボットは未だ望ましい姿勢（上体傾斜０）であったと仮定する。
【０００７】
提案した制御装置では、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントが検出される。この瞬間では、この実全床反力モーメントは、第１足平床反力の鉛直成分が過大であるため、ロボットを後に転倒させる方向に作用する。
【０００８】
このモーメントを０にしようと、図３４に示すごとく、仮想床面Ａ−Ａ’を想定し、各足平をあたかも仮想床面上に乗せたまま、仮想床面を目標全床反力中心点（目標ＺＭＰ）まわりに適当な角度Δθだけ回転させた位置に各足平の位置を移動させる。
【０００９】
それにより、第１足平床反力の鉛直成分が減少すると共に、第２足平床反力の鉛直成分が増大する。この結果、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントがほぼ０になる。即ち、床に予期しなかった斜面があっても、両脚コンプライアンス制御が正常に働くので、ロボットを転倒させないで歩行継続させることができる。
【００１０】
しかしながら、この提案技術だけでは両脚支持期に各足平実床反力を制御することができないので、足平の接地点あたりの床形状に予期しない局所的な傾きや凹凸があると、足平の接地性が低下してスピンしやすくなったり、急激な姿勢変化を起こして転倒する場合がある。
【００１１】
例えば、図３５に示すように、両脚支持期に第１足平のつまさきが予期しない突起（段差）を踏んでしまうと、両脚支持期は、第１足平のつまさきが急激に下がりつつある時期であるので、つまさきで床を強く蹴ってしまい、第１足平床反力の鉛直成分が急増する。その結果、目標全床反力中心点（目標ＺＭＰ）まわりに急激に実全床反力モーメントが発生し、最悪の場合、両脚コンプライアンスによって姿勢を復元させようとしても間に合わずに転倒する。
【００１２】
また、両脚支持期で倒れなかったとしても、その直後に第２足平を床から離したとき、目標全床反力中心点（目標ＺＭＰ）は第１足平のかかとにあるにもかかわらず、かかとが浮いているために実全床反力中心点はつまさきにあるので、目標全床反力中心点（目標ＺＭＰ）まわりにロボットを後に倒そうとする実全床反力モーメントが発生し、転倒する。
【００１３】
即ち、この両脚コンプライアンス制御は、長い距離でゆったりと変化する大域的な傾斜やうねりには対応できるが、足平の着地点の局所的な傾きや段差には対応できないと言える。
【００１４】
上記した両脚コンプライアンス制御とは別に、本出願人は、例えば特開平５−３０５５８４号公報において、２足歩行ロボットの足首部にゴムなどのばね特性を持った着地衝撃吸収機構を備えると共に、各足首まわりの実足平床反力モーメント成分を検出し、それを０にしようと各足首を回転させる足首コンプライアンス制御を提案している。
【００１５】
上記した問題点を解決するため、両脚コンプライアンス制御に加えて、この特開平５−３０５５８４号公報で提案する技術（以下『足首コンプライアンス制御』という）を併用することもできる。
【００１６】
その結果、足首コンプライアンス制御によって、図３６に示すように、予期しなかった第１足平床反力モーメントを打ち消す方向に第１足首を回転させ、かかとも床に接地させることができる。従って、その後の片脚支持期になっても上述のようにロボットを転倒させることはない。
【００１７】
【発明が解決しようとする課題】
しかしながら、上記した両脚コンプライアンス制御および足首コンプライアンス制御を単純に併用するだけでは、２種の制御が干渉しあい、実全床反力と実各足平床反力が望ましい値からずれたり発振してしまう問題があった。
【００１８】
従って、この発明の目的は上記した不都合を解消することにあり、脚式移動ロボットに作用する実床反力を、干渉を生じることなく、容易かつ適切に制御することができる脚式移動ロボットの制御装置を提供することにある。
【００１９】
さらに、ロボットの実各足平床反力の力成分を独立に制御すれば、例えばロボットを進行方向に推進させる力を、接地圧の高い足平には大きくすると共に、低い足平には小さくすることによって、姿勢安定性を向上させてスリップを効果的に防止することができる。また、ロボットの実各足平床反力の力成分を適正に制御することで、一方の足平が他方の足平の発生する進行方向に推進させる力の負荷となっている状態なども解消させることができ、関節アクチュエータの負担を軽減させることができる。
【００２０】
従って、この発明の第２の目的は、ロボットの実各足平床反力の力成分を独立に制御することで、姿勢安定性を向上させてスリップを効果的に防止すると共に、関節アクチュエータの負担を軽減させるようにした脚式移動ロボットの制御装置を提供することにある。
【００２１】
この発明の第３の目的は、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる脚式移動ロボットの制御装置を提供することにある。
【００２２】
この発明の第４の目的は、脚式移動ロボットに作用する床反力をさらに制御することによって、脚式移動ロボットの姿勢安定化制御を容易にすることができる脚式移動ロボットの制御装置を提供することにある。
【００２３】
この発明の第５の目的は、さらに、脚式移動ロボットに作用する床反力を適切に制御することによって、脚式移動ロボットの接地性を高め、歩行時のスリップや前述のスピンを防止することができる脚式移動ロボットの制御装置を提供することにある。
【００２４】
【課題を解決するための手段】
上記の目的を達成するために、請求項１項にあっては、少なくとも基体と、前記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、前記足部に作用する実床反力を検出する実床反力検出手段、前記決定された目標足部床反力中心点まわりに前記検出された実床反力が作用するモーメントを算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる足部回転量を決定する足部回転量決定手段、前記検出された実床反力から実全床反力に影響しない内力成分を求めて前記生成された歩容の内力成分との偏差を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位を決定する補償変位決定手段、前記決定された足部回転量に基づいて前記足部の位置または姿勢を回転させつつ、前記決定された補償変位に基づいて前記足部の目標位置または姿勢を移動させることによって、前記足部の目標位置または姿勢を修正する足部位置・姿勢修正手段、および前記足部位置・姿勢修正手段によって修正された前記足部の目標位置・姿勢に基づいて前記第１および第２の関節を変位させる関節変位手段を備える如く構成した。
【００２５】
請求項２項にあっては、少なくとも基体と、前記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、前記足部に作用する実床反力を検出する実床反力検出手段、少なくとも前記検出された実床反力に基づいて前記足部を回転させる足部回転量を決定する足部回転量決定手段、前記検出された実床反力から実全床反力に影響しない内力成分を求めて前記生成された歩容の内力成分との偏差を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位を決定する補償変位決定手段、前記決定された足部回転量に基づいて前記足部の位置または姿勢を、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転させつつ、前記決定された補償変位に基づいて前期足部の目標位置または姿勢を移動させることによって、前記足部の目標位置または姿勢を修正する第２の足部位置・姿勢修正手段、および前記足部位置・姿勢修正手段によって修正された前記足部の位置または姿勢に基づいて前記第１および第２の関節を変位させる関節変位手段を備える如く構成した。
【００２６】
請求項３項にあっては、前記足部位置・姿勢修正手段は、さらに前記決定された足部回転量に基づいて前記足部の位置または姿勢が、前記決定された目標足部床反力中心点またはその近傍まわりに回転するように、前記足部の目標位置または姿勢を修正する如く構成した。
【００２７】
請求項４項にあっては、さらに、前記ロボットに実際に作用する全床反力モーメント、または前記ロボットに実際に作用する全床反力モーメントから前記足部に作用する床反力モーメントを減算して得たモーメントのいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる足部移動量を決定する足部移動量決定手段を備え、前記足部位置・姿勢修正手段は、前記決定された足部回転量、前記決定された補償変位および前記決定された足部移動量に基づいて前記足部の目標位置または姿勢を修正する如く構成した。
【００２８】
請求項５項にあっては、前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モーメントを求め、前記足部回転量決定手段または前記足部移動量決定手段は、少なくとも前記検出された実床反力と前記求めた姿勢安定化補償全床反力モーメントに基づいて前記足部回転量または足部移動量を決定する如く構成した。
【００２９】
請求項６項にあっては、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差に基づいて求める如く構成した。
【００３０】
請求項７項にあっては、前記補償変位決定手段は、前記偏差に所定の重みを乗じて前記足部の補償変位を足部のそれぞれに分配する如く構成した。
【００３１】
請求項８項にあっては、前記所定の重みが時変特性および周波数特性の少なくともいずれかを持つ如く構成した。
【００３２】
【作用】
請求項１項にあっては、脚式移動ロボットに作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に近い制御を行っても、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。
【００３３】
また、ロボットの実各足平床反力が作り出す内力成分（実全床反力に影響しない成分の組み合わせ）を独立に制御することで姿勢安定性を向上させてスリップを効果的に防止することができると共に、関節アクチュエータの負担を軽減させることができる。
【００３４】
さらには、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。
【００３５】
請求項２項にあっては、請求項１項と同様の作用効果を得ることができる。
【００３６】
請求項３項にあっては、請求項１項と同様の作用効果を得ることができると共に、床反力をより適切に制御することができる。
【００３７】
請求項４項にあっては、請求項１項と同様の作用効果を得ることができると共に、特に姿勢制御に重要な全床反力を一層適切に制御することができる。
【００３８】
請求項５項にあっては、請求項１項と同様の作用効果を得ることができると共に、姿勢安定化能力を向上させることができる。
【００３９】
請求項６項にあっては、前記したと同様の作用効果を得ることができると共に、姿勢安定化能力を一層向上させることができる。
【００４０】
請求項７項にあっては、前記したと同様の作用効果を得ることができる。
【００４１】
請求項８項にあっては、前記したと同様の作用効果を得ることができると共に、補償変位の分配を一層適正に行うことができる。
【００４２】
【発明の実施の形態】
以下、添付図面を参照してこの発明に係る脚式移動ロボットの制御装置を説明する。尚、脚式移動ロボットとしては２足歩行ロボットを例にとる。
【００４３】
図１はその脚式移動ロボットの制御装置を全体的に示す概略図である。
【００４４】
図示の如く、２足歩行ロボット１は左右それぞれの脚部リンク２に６個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。６個の関節は上から順に、股（腰部）の脚部回旋用の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、股（腰部）のロール方向（Ｘ軸まわり）の関節１４Ｒ，１４Ｌ、股（腰部）のピッチ方向（Ｙ軸まわり）の関節１２Ｒ，１２Ｌ、膝部のピッチ方向の関節１６Ｒ，１６Ｌ、足首のピッチ方向の関節１８Ｒ，１８Ｌ、同ロール方向の関節２０Ｒ，２０Ｌから構成される。
【００４５】
関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）の下部には足平（足部）２２Ｒ，２２Ｌが取着されると共に、最上位には上体（基体）２４が設けられ、その内部にマイクロコンピュータからなる制御ユニット２６（後述）などが格納される。上記において股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から、足関節（足首関節）は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２８Ｒ，２８Ｌ、膝関節と足関節とは下腿リンク３０Ｒ，３０Ｌで連結される。
【００４６】
上記の構成により、脚部リンク２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６＊２＝１２個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行させることができる（この明細書で「＊」はスカラに対する演算としては乗算を、ベクトルに対する演算としては外積を示す）。
【００４７】
尚、この明細書で後述する上体の位置およびその速度は、上体２４の所定位置、具体的には上体２４の重心位置などの代表点の位置およびその移動速度を意味する。
【００４８】
図１に示す如く、足関節の下方には公知の６軸力センサ４４が取着され、力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚとを測定し、足部の着地の有無および床反力（接地荷重）などを検出する。
【００４９】
また、上体２４には傾斜センサ６０が設置され、Ｚ軸（鉛直方向（重力方向））に対する傾きとその角速度を検出する。傾斜センサ６０はＧセンサおよび角速度センサとから構成され、Ｇセンサは上体２４のＸ，Ｙ，Ｚ軸方向の加速度を検出する。また各関節の電動モータには、その回転量を検出するロータリエンコーダが設けられる。
【００５０】
図２に示すように、足平２２Ｒ（Ｌ）の上方には、ばね機構３２が装備されると共に、足底にはゴムなどからなる足底弾性体３４が貼られる。ばね機構３２は具体的には、足平２２Ｒ（Ｌ）に取り付けられた方形状のガイド部材と、足首関節１８Ｒ（Ｌ）および６軸力センサ４４側に取り付けられ、前記ガイド部材に弾性材を介して微動自在に収納されるピストン状部材とからなる。
【００５１】
図中に実線で表示された足平２２Ｒ（Ｌ）は、床反力を受けていないときの状態を示す。床反力を受けるとバネ機構３２と足底弾性体３４がたわみ、足平は図中に点線で表示された位置・姿勢に移る。この構造は、着地衝撃を緩和するためだけでなく、制御性を高めるためにも重要なものである。尚、その詳細は前記した特開平５−３０５５８４号に記載されているので、詳細な説明は省略する。
【００５２】
更に、図１では図示を省略するが、２足歩行ロボット１の適宜な位置にはジョイスティック６２が設けられ、外部から必要に応じて直進歩行しているロボットを旋回させるなど歩容に対する要求を入力できるように構成される。
【００５３】
図３は制御ユニット２６の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ６０などの出力はＡ／Ｄ変換器７０でデジタル値に変換され、その出力はバス７２を介してＲＡＭ７４に送られる。また各電動モータに隣接して配置されるエンコーダの出力はカウンタ７６を介してＲＡＭ７４内に入力される。
【００５４】
制御ユニット内にはＣＰＵからなる第１、第２の演算装置８０，８２が設けられており、第１の演算装置８０は後述の如く、ＲＯＭ８４に格納されている歩容に基づいて後述の如く関節角変位指令を算出し、ＲＡＭ７４に送出する。また第２の演算装置８２はＲＡＭ７４からその指令と検出された実測値とを読み出し、各関節の駆動に必要な制御値を算出してＤ／Ａ変換器８６とサーボアンプを介して各関節を駆動する電動モータに出力する。
【００５５】
ここで、この明細書および図面で使用する用語について定義する（尚、定義しない用語に関しては、本出願人が前記した技術とは別に提案した出願（特願平８−２１４２６１号）で使用した定義に従う）。
【００５６】
『歩容』は、ロボット工学における一般的な定義と異なり、目標運動パターンと床反力パターンを合わせたものを指称する意味で使用する。但し、床反力パターンとしては、例えば『ＺＭＰ軌跡だけ』というように、部分情報であっても良い。そのため、目標運動パターンだけを出力して床反力パターンに関する情報を出力しない装置に対して「歩容生成装置」と言う言葉を用いない。
【００５７】
各脚には、通し番号をつける。第ｎ脚に作用する床反力を第ｎ足平床反力という（ｎ：１または２。以下同じ）。全脚に作用する床反力を合成したものを全床反力という（ロボット工学では一般的には床反力と呼ばれるが、足平床反力と区別するためにここでは『全床反力』という）。
【００５８】
足平床反力は作用点とそこにかかる力と力のモーメントによって表現され、同一の足平床反力に対して、表現の組み合わせは無限通りある。その中には、鉛直軸まわりの成分を除くモーメント成分が０でかつ作用点が床面上にある表記が存在する。この表現における作用点を、ここでは足平床反力中心点という（本出願人が別途提案した後述する特開平６−７９６５７号では『接地圧重心点』と称した）。
【００５９】
同様に、全床反力は作用点とそこにかかる力と力のモーメントによって表現され、同一の全床反力に対して表現の組み合わせは無限通りある。その中には、鉛直軸まわりの成分を除くモーメント成分が０でかつ作用点が床面上にある表現が存在する。この表現における作用点を、ここでは全床反力中心点という。
【００６０】
全床反力の目標値を目標全床反力という。目標全床反力は、通常、目標運動パターンに対して動力学的に平衡する全床反力である。従って、通常、目標全床反力中心点は、目標ＺＭＰに一致する。
【００６１】
尚、始めに触れたように、目標ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ ）は次のように定義される。即ち、目標運動パターンによって発生する慣性力と重力の合力を動力学的に求め、これが床面上のある点に作用するモーメントが、鉛直軸まわりの成分を除き０であるならば、その点を目標ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）という。目標ＺＭＰは、合力の垂直方向力成分が０でない限り、一義的に求められる。以下の説明では、理解しやすくするために、目標ＺＭＰという言葉を用いる場合もあるが、厳密には目標全床反力中心点と言うべき箇所が多い。
【００６２】
各足平床反力の目標値を目標各足平床反力という。但し、目標全床反力とは異なり、目標運動パターンが決まっていても目標各足平床反力は一義的には決定されない。実際のロボットに作用する全床反力を実全床反力という。実際のロボットに作用する各足平床反力を実各足平床反力という。
【００６３】
ここで、この発明の課題について再説すると、この発明は、局所的な傾きや段差に対して先に提案した両脚コンプライアンス制御では良好な姿勢安定性を得ることが困難であると共に、その不都合は足首コンプライアンス制御を用いれば解消することができるが、両者を単純に併用するだけでは干渉し合い、実全床反力と実各足平床反力が望ましい値からずれたり、発振する不都合があった。
【００６４】
その問題点を先の図３３に示す状況で説明すると、第１足平はかかとに予期しなかった過大な床反力を受けているため、第１足首のまわりに過大な実足平床反力モーメントが発生する。足首コンプライアンス制御は、このモーメントを０にしようと第１足首を図３６に示すように回転させる。
【００６５】
しかしながら、足首の回転によって、第１足平のかかと位置が高くなるので、第１足平床反力の鉛直成分が減少する。この結果、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントが変化する。これは、両脚コンプライアンス制御の制御量である実全床反力モーメントが、足首コンプライアンス制御に干渉されることを意味する。
【００６６】
従って、足首コンプライアンス制御による干渉を考慮しないで、両脚コンプライアンス制御を、足首コンプライアンス制御がない場合と同様に働かせると、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントが０からずれたり、干渉による振動や発振が生じる。
【００６７】
それを防止する方法のひとつとして、両脚コンプライアンス制御と足首コンプライアンス制御の間の干渉量を求め、それを打ち消すような操作量を加えることによって干渉しないようにすることが考えられるが、歩行中は姿勢が時々刻々と変化し、干渉関係も時々刻々と変化するため、この手法で干渉を回避することは極めて難しい。
【００６８】
また、図３６に示す状況では、第１足平が接触している床は想定していた床よりも登り傾斜なので、第１足平は、目標歩容よりもつまさきを上げるべきである。それにもかかわらず、足首コンプライアンス制御によりつまさきが下がってしまうことは、足首コンプライアンス制御が適切に作用していないとも言える。
【００６９】
以上のように、足首コンプライアンス制御は足平の着地点の局所的な床の傾きや段差には効果があるが、長い距離でゆったりと変化する傾斜やうねりには、却って悪影響を与える場合がある。
【００７０】
従って、実施の形態に係る装置においては、脚式移動ロボットに作用する床反力、より具体的には、目標全床反力中心点まわりの実全床反力モーメントと、目標各足平床反力中心点まわりの実各足平床反力モーメントを容易かつ適切に制御できるようにした。
【００７１】
また、それによって大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに安定した姿勢でロボットを歩行継続させるようにした。
【００７２】
尚、上記した課題を解決すべく、本出願人は先に脚式移動ロボットの制御装置を提案しており、この発明の目的はその提案技術をさらに改良し、実各足平床反力の力成分を独立に制御して姿勢安定性を向上させてスピンなどを防止すると共に、関節アクチュエータの負担も軽減するようにした脚式移動ロボットの制御装置を提供することにある。
【００７３】
図４は、この実施の形態に係る脚式移動ロボットの制御装置（主として図３の第１の演算装置８０に相当）の構成および動作を機能的に示すブロック図である。以下、図４を参照してこの装置の全体構成を概説する。
【００７４】
この装置は歩容生成器を備え、歩容生成器は目標歩容を生成し、出力する。目標歩容は、前述の定義の通り、目標運動パターンと目標床反力パターン、より具体的には目標上***置・姿勢軌道、目標足平位置・姿勢軌道、目標全床反力中心点（目標ＺＭＰ）軌道および目標全床反力軌道からなる。目標床反力パターンは、このように、目標全床反力中心点軌跡を含む（後述する機構変形補償を行わないならば、目標床反力パターンとしては目標全床反力中心点軌跡だけでも良い）。
【００７５】
この実施の形態において歩容生成器が出力する目標全床反力は、目標運動パターンに対して動力学的に平衡する全床反力である。従って、目標全床反力中心点は、目標ＺＭＰに一致する。
【００７６】
図５にロボット１が平地を歩行するときの目標運動パターンの一例を示す。これに対応する目標ＺＭＰ軌道の床面上軌跡を図６に、タイム・チャートを図７に示す。この歩容の期間に床に接触したままの足平を、第１足平、もう一方を第２足平ということとする。尚、歩容生成器の詳細は先に提案した特願平８−２１４２６１号に詳細に述べられているので、これ以上の説明は省略する。
【００７７】
図４の説明に戻ると、この装置は目標床反力分配器を備え、目標床反力分配器は、目標全床反力中心点（目標ＺＭＰ）と目標足平位置・姿勢を主な入力とし、目標各足平床反力中心点を決定して出力する。実際には、歩容生成器から歩容のパラメータ（例えば、両脚支持期の時間や遊脚足平の目標着地位置など）や、歩容の時期・時刻（例えば、現在時刻が両脚支持期の初めから０．１ｓｅｃであるなど）などの情報も必要に応じて取り込む。
【００７８】
図５に示すような歩容に対して、目標床反力分配器は、目標各足平床反力中心点が以下の条件を満足するように設定する。
条件１）目標各足平床反力中心点軌跡は連続である。
条件２）両脚支持期では、目標第１足平床反力中心点はかかとに、目標第２足平床反力中心点はつまさきに存在する。
条件３）このとき目標第１足平床反力中心点と目標第２足平床反力中心点を結ぶ線分上に、目標全床反力中心点が存在する。
条件４）片脚支持期では、目標第１足平床反力中心点は、目標全床反力中心点に一致する。
条件５）片脚支持期の間に、目標第２足平床反力中心点は、つまさきからかかとに移動する。
【００７９】
これら条件を満足する目標第１足平床反力中心点軌跡のタイム・チャートを図８に、目標第２足平床反力中心点軌跡のタイム・チャートを図９に示す。尚、この図では足首（関節１８，２０Ｒ（Ｌ））から足平２２Ｒ（Ｌ）への垂直投影点を原点とし、図１に示すように足平前方向をＸ軸の正の向き、足平左方向をＹ軸の正の向きにとる。
【００８０】
目標床反力分配器は、更に、付随的ではあるが、目標各足平床反力も決定して出力する。目標各足平床反力は、ばね機構３２などのたわみ補償のために必要である。
【００８１】
次式を用いて上記のように設定された目標各足平床反力中心点に対応する目標各床反力を決定すれば、目標各足平床反力の合力は目標全床反力に一致しなければならないと言う条件を満足する。
【００８２】
このように求めた目標各足平床反力は連続的に変化するので、衝撃の少ない歩行を実現するために適している。尚、上記の詳細は本出願人が別途提案した技術（特開平６−７９６５７号）に記述されている。
【００８４】
図４の説明に戻ると、この装置は姿勢安定化制御演算部を備え、姿勢安定化制御演算部はロボットのセンサ情報に基づいてロボットの状態を推定し、補償全床反力を算出する。即ち、実際にロボットが歩行あるいは直立しているときなどには後述する変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたとしても、ロボットの位置・姿勢は必ずしも望ましい位置・姿勢にならない。
【００８５】
ロボットの姿勢を長期的に安定化させるためには、ロボットを望ましい位置・姿勢に復元させるために必要な力とモーメントを求め、これを目標全床反力中心点（目標ＺＭＰ）を作用点として付加的に発生させる必要がある。この付加的な力とモーメントを補償全床反力という。また、補償全床反力のモーメント成分を補償全床反力モーメントという。
【００８６】
尚、脚式移動ロボットの目標歩容が床反力以外の反力を環境から受けるように想定し、それを例えば、目標対象物反力と称し、先に述べた目標ＺＭＰの定義を次のように拡張しても良い。即ち、目標運動パターンによって発生する慣性力と重力と目標対象物反力の合力を動力学的に求め、それが床面上のある点に作用するモーメントが、鉛直軸まわりの成分を除いて零であるならば、その点を改めて目標ＺＭＰとするようにしても良い。
【００８７】
もし、ロボット１が完全剛体であって、変位コントローラによって実関節変位を目標関節変位に完全に追従させることができたと仮定すると、足平のばね機構３２および足底弾性体３４のたわみによって生じるロボット全体の位置・姿勢の摂動的な運動は、以下の６自由度に分解できる。
【００８８】
モード１）目標全床反力中心点（目標ＺＭＰ）を中心とした前後軸まわり回転（即ち、左右傾き）
モード２）目標全床反力中心点（目標ＺＭＰ）を中心とした左右軸まわり回転（即ち、前後傾き）
モード３）目標全床反力中心点（目標ＺＭＰ）を中心とした鉛直軸まわり回転（即ち、スピン）
モード４）前後平行移動揺れ
モード５）左右平行移動揺れ
モード６）上下平行移動揺れ
【００８９】
この内で、モード４とモード５は、足平のばね機構３２および弾性体３４が前後左右方向のせん断力を受けて撓むことによって発生するものである。ばね機構３２および足底弾性体３４は剪断方向の剛性が高いように製作するので、この揺れは極めて少なく、歩行に及ぼす悪影響はほとんどない。
【００９０】
先に提案した技術では主としてモード１とモード２を扱っていたが、この発明に係る実施の形態では、それに加えて、ロボット全体の挙動（モード１から６）に直接的には影響しない内力の制御を行う。ここでいう「内力」とは、全床反力に影響を与えない各足平床反力の成分の組み合わせを意味する。尚、理解の便宜のため、重複するが、先の提案技術を説明する。
【００９１】
モード１を制御するための操作量は、補償全床反力の前後軸（Ｘ軸）まわりモーメント成分である。モード２を制御するための操作量は、補償全床反力の左右軸（Ｙ軸）まわりモーメント成分である。従って、補償全床反力の成分の内、前後軸方向モーメント成分と左右軸方向モーメント成分だけを求めれば良い。他の成分は、この実施の形態では用いないので０で良い。
【００９２】
尚、以降は次の定義に従う。即ち、補償全床反力のモーメント成分を補償全床反力モーメントＭｄｍｄ（詳しくは目標全床反力中心点（目標ＺＭＰ）まわりの補償全床反力モーメントＭｄｍｄ）という。図５に示す如く、ロボットの前方向をＸ軸、左横方向をＹ軸、上方向をＺ軸にとり、第１足平の足首直下の床面上の点を原点とした座標系を支持脚座標系と呼び、断らない限り、位置、力およびモーメントはこの座標系で表現されるものとする。また、ＭｄｍｄのＸ成分をＭｄｍｄｘ、Ｙ成分をＭｄｍｄｙ、Ｚ成分をＭｄｍｄｚと記述する。上体２４の傾斜偏差（即ち、実上体傾斜−目標上体傾斜）θｅｒｒ のＸ成分をθｅｒｒｘ， Ｙ成分をθｅｒｒｙ、これらの時間微分値を（ｄθｅｒｒｘ ／ ｄｔ），（ｄθｅｒｒｙ ／ ｄｔ）と記述する。
【００９３】
ＭｄｍｄｘおよびＭｄｍｄｙは、例えば次式の制御則によって決定される。
【００９４】
後述する複合コンプライアンス動作決定部は、目標全床反力と補償全床反力の合力に実全床反力を一致させようと働く。
【００９５】
図４の説明に戻ると、この装置は実各足平床反力検出器を備え、実各足平床反力検出器は、６軸力センサ４４によって実各足平床反力（その合力が実全床反力）を検出する。更に、関節のエンコーダによって検出される実変位（あるいは変位指令）に基づき、上体に固定された座標系に対する各足平の相対位置・姿勢を算出し、それによって６軸力センサ４４の検出値を座標変換し、上体に固定された座標系で表現された実各足平床反力を算出した後、更に、支持脚座標系に変換する。
【００９６】
この装置はロボット幾何学モデル（逆キネマティクス演算部）を備え、ロボット幾何学モデルは、上***置・姿勢と足平位置・姿勢を入力されると、それらを満足する各関節変位を算出する。この実施の形態におけるロボット１のような１脚あたりの関節自由度が６である場合には、各関節変位は一義的に求まる。
【００９７】
この実施の形態では逆キネマティクスの解の式を直接的に求めておき、式に上***置・姿勢と足平位置・姿勢を代入するだけで各関節変位を得るようにした。即ち、ロボット幾何学モデルは、目標上***置・姿勢と複合コンプライアンス動作決定部で修正された修正目標足平位置・姿勢軌道（機構変形補償入り修正目標足平位置・姿勢軌道）を入力し、それらから１２個の関節（１０Ｒ（Ｌ）など）の関節変位指令（値）を算出する。尚、ヤコビアンによって関節角変位を得ても良い。
【００９８】
この装置は変位コントローラ（前記した第２の演算装置８２に同じ）を備え、変位コントローラは、ロボット幾何学モデル（逆キネマティクス演算部）で算出された関節変位指令（値）を目標値としてロボット１の１２個の関節の変位を追従制御する。
【００９９】
前記した複合コンプライアンス動作決定部は、以下の２つの要求を満足させようと、目標足平位置・姿勢軌道を修正する。
【０１００】
要求１）ロボットの位置・姿勢制御のために、実全床反力を姿勢安定化制御部が出力する補償全床反力（モーメントＭｄｍｄ）と目標全床反力の合力に追従させる。ロボットの姿勢傾きだけを制御したい場合には、目標全床反力中心点まわりの実全床反力水平方向モーメント成分だけを補償全床反力モーメントＭｄｍｄに追従させる。
【０１０１】
要求２）各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。
【０１０２】
尚、補足すると、通常は実全床反力を補償全床反力と目標全床反力の合力に一致させながら目標各足平床反力中心点まわりの実各足平床反力モーメントを０にすることが、物理的に不可能な場合が多い。従って、要求１）と要求２）は完全に両立させることはできず、ある点で妥協しなくてはならない。
【０１０３】
上記を前提として図１０フロー・チャート（構造化フロー・チャート）を参照してこの装置の動作を説明する。尚、図の左端に該当する処理を行う図４装置の構成要素を示す。
【０１０４】
先ずＳ１０において装置を初期化し、Ｓ１２を経てＳ１４に進み、タイマ割り込みを待機する。タイマ割り込みは５０ｍｓごとになされ、即ち、制御周期は５０ｍｓである
【０１０５】
続いてＳ１６に進んで歩容の切り替わり目、即ち、支持脚の切り替わり目か否か判断し、否定されるときはＳ２２に進むと共に、肯定されるときはＳ１８に進んでタイマｔをイニシャライズし、Ｓ２０に進んで目標歩容パラメータを設定する。前記の如く、歩容パラメータは、運動パラメータと床反力パラメータ（ＺＭＰ軌道パラメータ）から構成される。
【０１０６】
続いてＳ２２に進み、目標歩容の瞬時値を決定する。ここで『瞬時値』は制御周期ごとの値を意味し、目標歩容瞬時値は、目標上***置・姿勢、目標各足平位置・姿勢、および目標ＺＭＰ位置から構成される。尚、ここで『姿勢』はＸ，Ｙ，Ｚ空間における『向き』を意味する。
【０１０７】
続いてＳ２４に進んで目標各足平床反力中心点を求める。これは、目標床反力分配器の説明で述べたように行う。具体的には、図８および図９に示すように設定した目標各足平床反力中心点軌跡の現在時刻ｔにおける値を求めることで行う。
【０１０８】
続いてＳ２６に進んで目標各足平床反力を求める。これは目標床反力分配器の説明で述べた式１を用いて目標各足平床反力を演算することで行う。
【０１０９】
続いてＳ２８に進み、前記した傾斜センサ６０などの出力から上体２４の傾斜などロボット１の状態を検出する。
【０１１０】
続いてＳ３０に進み、ロボット１の状態などから姿勢を安定化するための（目標全床反力中心点（目標ＺＭＰ）まわりの）補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを求める。具体的には、上体傾斜が検出されたとき姿勢安定化を図るために前記した式２に従って補償全床反力モーメントＭｄｍｄｘ，Ｍｄｍｄｙを演算する。
【０１１１】
続いてＳ３２に進んで実各足平床反力を検出する。これは前記の如く、６軸力センサ４４の出力から検出する。
【０１１２】
続いてＳ３４に進み、両脚補償角θｄｂｖおよび各足平補償角θｎｘ（ｙ）を決定する。これは、前記した複合コンプライアンス動作決定部が行う作業である。
【０１１３】
その複合コンプライアンス動作決定部の作業について説明する。説明の便宜のため、両脚支持期において図１１に示すように第１足平２２Ｒ（Ｌ）と第２足平２２Ｌ（Ｒ）に実各足平床反力が作用している状況と仮定する。
【０１１４】
ここでベクトルＦｎａｃｔは第ｎ足平床反力の力成分を表す。ベクトルＭｎａｃｔは第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎａｃｔの向きは、向きに対して時計回りのモーメントが床から足平に作用していることを表す。
【０１１５】
この瞬間の目標全床反力は、図１２に示すようになっていると仮定する。ちなみに、目標全床反力中心点（目標ＺＭＰ）における目標全床反力モーメントベクトルＭｓｕｍｒｅｆは垂直である（定義により、目標ＺＭＰは目標全床反力モーメントの水平方向成分が０である点であるから）。
【０１１６】
これを式１に従って目標各足平床反力に分配すると、図１３に示すようになる。同図において、ベクトルＦｎｒｅｆは目標第ｎ足平床反力の力成分を表す。ベクトルＭｎｒｅｆは目標第ｎ足平床反力のモーメント成分を表す。ベクトルＭｎｒｅｆの向きの表現は、Ｍｎａｃｔと同様である。
【０１１７】
説明のため、上体姿勢が左後ろに倒れそうな状態を想定する。
【０１１８】
前述の姿勢安定化制御演算部では、ロボット１の上体傾斜偏差検出値θｅｒｒｘ，θｅｒｒｙに基づいて補償全床反力モーメントＭｄｍｄを算出する。この実施の形態では鉛直軸（Ｚ軸）まわりのスピンを制御しないので、補償全床反力モーメントＭｄｍｄの鉛直軸成分は０である。上***置の揺れも制御しないので、補償全床反力モーメントの力成分も０である。この状態に対応する補償全床反力モーメントＭｄｍｄを図１４に示す。
【０１１９】
姿勢を復元させるためには、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平成分を、目標全床反力モーメントＭｓｕｍｒｅｆと補償全床反力モーメントＭｄｍｄの和の水平成分に追従させれば良い。
【０１２０】
一方、目標全床反力中心点（目標ＺＭＰ）では目標全床反力モーメントＭｓｕｍｒｅｆの水平方向成分は０である。従って、前後左右の姿勢傾きを復元させるためには、目標ＺＭＰまわりの実全床反力モーメントの水平成分を、Ｍｄｍｄの水平成分に追従させれば良い。
【０１２１】
この実施の形態にあっては複合コンプライアンス動作決定部は、以下の要求をできる限り満足するように足平の位置・姿勢を修正する。
要求１）ロボットの姿勢傾斜を安定化制御するために、目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平方向（Ｘ，Ｙ軸方向）成分を、補償全床反力モーメントＭｄｍｄの水平方向成分に追従させる。
要求２）各足平の接地性を確保するために、できるかぎり目標各足平床反力中心点まわりの実各足平床反力モーメントの絶対値を小さくする。
【０１２２】
但し、前述の通り、要求１）と要求２）は、完全に両立させることはできず、ある点で妥協しなくてはならない。
【０１２３】
足平の位置・姿勢の修正は、この実施の形態では次のように行う。
１）目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を含み、かつ水平面と垂直な平面の法線ベクトルＶを求める。Ｖの大きさは１とする。Ｖを図１５に示す。
【０１２４】
２）目標第１足平床反力中心点Ｑ１の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに、ある回転角θｄｂｖだけ回転移動する。移動した後の点をＱ１’とする。同様に、目標第２足平床反力中心点Ｑ２の座標を、目標全床反力中心点（目標ＺＭＰ）を回転中心に法線ベクトルＶまわりに回転角θｄｂｖだけ回転移動する。移動した後の点をＱ２’とする。
【０１２５】
この回転角θｄｂｖを両脚補償角という。始点がＱ１、終点がＱ１’のベクトルをベクトルＱ１Ｑ１’とする。同様に、始点がＱ２、終点がＱ２’のベクトルをベクトルＱ２Ｑ２’とする。図１６にＱ１’とＱ２’を示す。
【０１２６】
３）目標第１足平を、姿勢は変えずにベクトルＱ１Ｑ１’だけ平行移動（ほぼ上下移動）させる。同様に、目標第２足平を、姿勢は変えずにベクトルＱ２Ｑ２’だけ平行移動させる。移動後の目標各足平を図１６に太線で示す。
【０１２７】
４）次に、目標第１足平をＱ１’を中心に、前後方向軸（Ｘ軸）まわりに回転角θ１ｘ、左右方向軸（Ｙ軸）まわりに回転角θ１ｙだけ回転させる。同様に、目標第２足平を目標第２足平をＱ２’を中心に前後方向軸（Ｘ軸）まわりに回転角θ２ｘ、左右方向軸（Ｙ軸）まわりに回転角θ２ｙだけ回転させる。回転角θｎｘ，θｎｙをそれぞれ第ｎ足平Ｘ補償角、第ｎ足平Ｙ補償角という。回転後の目標各足平を図１７に太線で示す。
【０１２８】
以上の補償動作量が過大でなければ、接地圧力分布は変わっても、接地領域（足底面の圧力が正の領域）は変わらない。このような場合には、補償動作量に比例して各足平に装着されたばね機構３２や足底弾性体３４などが変形し、変形量に応じた実各足平床反力が発生する。この結果、補償動作量と補償動作によって発生する実床反力の変化量との間の関係は、以下に示す良好な特性を持つ。
【０１２９】
特性１）両脚補償角θｄｂｖだけを操作して目標各足平位置を移動させると、下がった足平の実足平床反力の力成分が増加し、上がった足平の実足平床反力の力成分が減少する。このとき、修正目標各足平床反力中心点まわりの実各足平床反力モーメントは、ほとんど変化しない。
【０１３０】
特性２）第ｎ足平Ｘ補償角だけを操作して目標第ｎ足平姿勢を回転させると、目標第ｎ足平床反力中心点に作用する実第ｎ足平床反力のモーメントのＸ成分だけが変化し、その他の床反力成分は少ししか変化しない。同様に、第ｎ足平Ｙ補償角だけを操作して目標第ｎ足平姿勢を回転させると、実第ｎ足平床反力のモーメントのＹ成分だけが変化し、その他の床反力成分は少ししか変化しない。
【０１３１】
特性３）両脚補償角θｄｂｖ、各足平Ｘ補償角および各足平Ｙ補償角を同時に操作すると、実各足平床反力の変化量は、それぞれを単独に操作したときの変化量の和になる。
【０１３２】
特性１および特性２は、これらの操作に独立性があることを示し、特性３はこれらの操作に線形性があることを示していると言える。
【０１３３】
図１８は複合コンプライアンス動作決定部の演算処理を示すブロック図であり、同図を参照してこの作業を説明する。
【０１３４】
概説すると、補償全床反力モーメント分配器において補償全床反力モーメントＭｄｍｄの分配を行う。次に、実各足平床反力と分配された補償全床反力モーメントなどから、両脚補償角決定部および第ｎ足平Ｘ（Ｙ）補償角決定部において前述の補償角θｄｂｖおよびθｎｘ（ｙ）を決定する。
【０１３５】
次に、決定された各種補償角に基づいて修正目標足平位置・姿勢算出部は、補償された足平位置・姿勢（これを修正目標足平位置・姿勢という）を幾何学演算によって求める。最後に、機構変形補償入り修正目標足平位置・姿勢算出部は、目標各足平床反力によって発生が予想されるばね機構３２や足底弾性体３４の変形量を求め、それらを打ち消すように修正目標足平位置・姿勢をさらに修正する。
【０１３６】
以下詳説すると、補償全床反力モーメント分配器は、補償全床反力モーメントＭｄｍｄを、両脚補償モーメントＭｄｍｄｄｂ、各足平補償モーメントＭｄｍｄ１ｘ，ｙ，Ｍｄｍｄ２ｘ，ｙに分配する。両脚補償モーメントＭｄｍｄｄｂは、両脚補償角（足平上下量）θｄｂｖを操作することによって目標全床反力中心点（目標ＺＭＰ）まわりに各足平床反力の力成分が作るモーメントの目標値である。
【０１３７】
両脚補償モーメントＭｄｍｄｄｂのＶ方向まわりの成分をＭｄｍｄｄｂｖと記述する。尚、ベクトルＶは複合コンプライアンス動作決定部の説明で定義したベクトルである。Ｖに直交し、鉛直方向にも直交するベクトルをＵとすると、両脚補償モーメントＭｄｍｄｄｂのＵ方向成分Ｍｄｍｄｄｂｕは０に設定される。両脚補償角θｄｂｖを操作しても、床反力のＵ方向モーメント成分を発生することはできないからである。
【０１３８】
この実施の形態では補償全床反力モーメントＭｄｍｄの鉛直方向成分が０なので、Ｍｄｍｄｄｂの鉛直方向成分Ｍｄｍｄｄｂｚも０に設定される。
【０１３９】
第１足平補償モーメントＭｄｍｄ１は、第１足平補償角θ１ｘ，θ１ｙを操作することによって目標第１足平床反力中心点まわりに発生させたいモーメントである。第１足平補償モーメントＭｄｍｄ１のＸ成分をＭｄｍｄ１ｘ、Ｙ成分をＭｄｍｄｌｙと記述する。第２足平補償モーメントＭｄｍｄ２は、第２足平補償角θ２ｘ，θ２ｙを操作することによって目標第２足平床反力中心点まわりに発生させたいモーメントである。第２足平補償モーメントＭｄｍｄ２のＸ成分をＭｄｍｄ２ｘ、Ｙ成分をＭｄｍｄ２ｙと記述する。
【０１４０】
分配は、例えば次のように行う。
ここで、Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔは分配用重み変数である。ＶｘはベクトルＶのＸ成分の値、ＶｙはベクトルＶのＹ成分の値である。この中で、Ｗｉｎｔは、両脚補償角を操作することによって発生した全床反力モーメントを各足平補償角を操作することによって打ち消すためのものである。
【０１４２】
式３と式４の演算処理を行う補償全床反力モーメント分配器のブロック図を図１９に示す。
【０１４３】
歩行時の分配用重み変数Ｗｄｂｘ，Ｗｄｂｙ，Ｗ１ｘ，Ｗ１ｙ，Ｗ２ｘ，Ｗ２ｙおよびＷｉｎｔの設定例を図２０に示す。図２０のパターンは、以下の注意点を考慮して決定することが望ましい。
【０１４４】
注意点１）両脚補償角と各足平補償角が不連続的に変化すると、関節に過大なトルクが発生する。そこで、両脚補償角と各足平補償角を連続的に変化させるために、分配用重み変数は連続的に変化させる。
【０１４５】
注意点２）両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントが、なるべく補償全床反力モーメントＭｄｍｄに近い値になるように、分配用重み変数を決定する。
【０１４６】
この際、直立時や歩行時など状況に応じて以下に示すように設定方針を変えた方が良い。直立時などのように、両脚補償モーメントのＶ方向成分Ｍｄｍｄｄｂｖ、各足平補償モーメントＭｄｍｄ１，Ｍｄｍｄ２を忠実に実各足平床反力に発生させることができる状況では以下のように設定する。
【０１４７】
この状況では目標全床反力中心点（目標ＺＭＰ）まわりの実全床反力モーメントの水平方向成分を、補償全床反力モーメントＭｄｍｄの水平方向成分に一致させるために、（即ち、前述の複合コンプライアンス動作決定部に対する要求１を満足するために、）式５と式６の両方をなるべく満足するように重みを設定すべきである。
【０１４８】
これに式３、式４を代入すると、式５は式７に、式６は式８に変換される。
ＭｄｍｄｘとＭｄｍｄｙが任意の値を取っても、式７と式８が恒等的に成立するためには、式９、式１０、および式１１を同時に満足すれば良い。
【０１５１】
歩行時ではＭｄｍｄｄｂｖを目標にして両脚補償角θｄｂｖを操作して足平の位置を修正しても、実全床反力モーメントの発生量がＭｄｍｄｄｂｖに較べて不足する場合がある。例えば図２１のように両脚支持期の初期にロボットが後傾して第１足平が未だ着地していない状況では、θｄｂｖによって第１足平の位置を下げても、実床反力は変化しない。
【０１５２】
同様に、Ｍｄｍｄ２を目標にして第２足平補償角θ２を操作して第２足平の角度を修正しても、実床反力モーメントの増加量がＭｄｍｄ２に較べて不足する場合がある。例えば、図２２のように両脚支持期の後半にロボットが後傾している状況では、θ２によって第２足平のかかとを下げても実床反力は変化しない。
【０１５３】
従って、式５、式６を満足するように各重みを設定しても、複合コンプライアンス制御によって発生する実全床反力の増加量が補償全床反力モーメントＭｄｍｄに届かない場合がある。このようなことが生じる可能性が高い状況では、式５、式６の左辺の値を右辺で割った値を１より大きくすべきである。
【０１５４】
歩行時の分配用重み変数設定例である図２０では、Ｗｉｎｔを０に設定することによって、図２１の状況のように、両脚補償角θｄｂｖ を操作しても実全床反力モーメントが発生できなくなっても、各足平補償角を操作して不足分を補うようにした。
【０１５５】
好都合なことに、図２１のように後傾すると第２足平のかかとが結果的に下がって床に接地しやすくなるので、第２足平補償角を操作することによって実全床反力モーメントを発生させることができるようになる。
【０１５６】
また、後傾していないときには両脚補償角θｄｂｖを操作することによる実全床反力モーメントが発生するが、第２足平のかかとが床に接地しないので、第２足平補償角を操作しても実全床反力モーメントは発生しない。
【０１５７】
つまり、両脚補償角θｄｂｖが有効に働くときには各足平補償角が有効に働かず、各足平補償角が有効に働くときには両脚補償角θｄｂｖが有効に働かないので、結果的に両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントの総量は、ほぼ補償全床反力モーメントＭｄｍｄに等しくなる。
【０１５８】
状況によっては、両脚補償角および各足平補償角を操作することによって発生する実床反力モーメントの総量が補償全床反力モーメントＭｄｍｄよりも大きくなってしまう場合がある。
【０１５９】
しかし、この場合でも、Ｍｄｍｄがこの実施の形態のように姿勢安定化のためのフィードバック操作量であるならば、あまり問題にならない。何故ならば、Ｍｄｍｄの大きさが多少違っていても、一般的に制御系に言えることであるが、制御系のオープンループゲインが多少変化するだけで、クローズドループ特性はほとんど変わらないからである。
【０１６０】
注意点３）片脚支持期では、両脚補償角用の分配用重み変数であるＷｄｂｘ，Ｗｄｂｙの絶対値を小さくする。片脚支持期では両脚補償角を変化させても、接地していない足平が無駄に上下するだけで、実各足平床反力は変化しないからである。
【０１６１】
注意点４）足平の接地性を確保するために、目標足平床反力の力成分が小さいときには、その足平の足平補償角のための分配用重み変数の絶対値を小さくする。特に、足平が床から遠く離れているときには、その足平の足平補償角を動かしても、その足平の実足平床反力は変化しないので、不要な動きをさせないためにも、その足平の足平補償角のための分配用重み変数の絶対値を小さくすべきである。
【０１６２】
注意点５）両脚補償角を操作することによって制御できる実全床反力モーメントの方向と、各足平補償角を操作することによって制御できる実全床反力モーメントの方向は通常異なる。
【０１６３】
例えば、両脚補償角θｄｂｖを操作することによって発生する実全床反力モーメントの向きは必ずＶ方向であり、Ｖ方向に直交する成分を発生させることはできない。一方、各足平補償角を操作することによって発生できる実全床反力モーメントの向きは、足平の接地状況によって制約を受ける。
【０１６４】
例えば、つまさきのエッジだけまたはかかとのエッジだけが接地している場合には、エッジ線方向にモーメントを発生することはできない。両脚支持期では、この特性を考慮して、なるべく無駄なく両脚補償角および各足平補償角を操作する。
【０１６５】
例えば、両脚補償角を操作するための分配重みＷｄｂｘ，Ｗｄｂｙは次のように決定する。
【０１６６】
Ｘ成分がＷｄｂｘ、Ｙ成分がＷｄｂｙ、Ｚ成分が０のベクトルをＷｄｂとすると、式３はベクトルＷｄｂとＭｄｍｄの内積になっている。従って、ＭｄｍｄをベクトルＷｄｂ方向成分とその直交成分に分解し、ベクトルＷｄｂ方向成分だけを抽出して、ベクトルＷｄｂの大きさを乗じたものが、式３によって求められるＭｄｍｄｄｂｖであると言える。
【０１６７】
この場合のＭｄｍｄｄｂｖを図２３に示す。これは、両脚補償角を操作することによって実全床反力モーメントのＷｄｂ方向成分を制御するフィードバック制御系を構成することを意味する。もし、Ｗｄｂ方向がベクトルＶと直交していたら、両脚補償角をいくら操作しても実全床反力モーメントのＷｄｂ方向成分は発生しないから、このフィードバック制御系はただ無駄に両脚補償角を操作するだけになる。
【０１６８】
従って、無駄な動きを減らした場合には、Ｗｄｂ方向をベクトルＶ方向に一致させるか、またはなるべく近づけるべきである。また、補償全床反力モーメントＭｄｍｄのＷｄｂ方向成分を、各足平補償角に頼らずに両脚補償角を操作するだけで発生させたいならば、ＷｄｂとＶの内積が１になるように設定する。一部を各足平補償角に頼らせたいならば、ＷｄｂとＶの内積が１より小さくなるように設定する。
【０１６９】
ところで、足平の横幅が狭い場合には、各足平補償角を操作することによって発生し得る実各足平床反力モーメントのＸ成分は小さくなる。この場合には、Ｗｄｂｘを大きめに設定する。Ｗｄｂ方向とベクトルＶ方向は一致しなくなり、両脚補償角の変動が増加するが、安定性が増す。
【０１７０】
両脚補償角決定部について更に詳説すると、図２４は両脚補償角決定部の演算処理のブロック図であり、両脚補償角θｄｂｖは図示の如く演算される。
【０１７１】
図２４を参照して説明すると、目標第１足平床反力中心点Ｑ１に作用するＦ１ａｃｔと目標第２足平床反力中心点Ｑ２に作用するＦ２ａｃｔが、目標全床反力中心点Ｐのまわりに発生させるモーメントＭｆ１ｆ２ａｃｔを、次式により求める。
【０１７２】
【０１７３】
また、式１２の代わりに、次式を用いても実際上はほとんど問題がない。
【０１７４】
次に、Ｍｆ１ｆ２ａｃｔのベクトルＶ方向成分Ｍｆ１ｆ２ａｃｔｖを抽出する。これは、ベクトルの内積演算を用いた次式によって得られる。尚、ベクトルＶは前述の動作説明において図１５に示したＶである。
次に、Ｍｆ１ｆ２ａｃｔｖをローパスフィルタに通してＭｆ１ｆ２ａｃｔｖｆｉｌｔを得る。
【０１７６】
次に、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを補償用フィルタに通し、それを、Ｍｆ１ｆ２ａｃｔｖｆｉｌｔから減じ、偏差モーメントＶ方向成分Ｍｄｉｆｆｖを得る。
【０１７７】
尚、補償用フィルタは、Ｍｄｍｄｄｂｖから実全床反力モーメントまでの伝達関数の周波数応答特性を改善するものである。
【０１７８】
次に、足平ばね機構などの変形による両脚補償モーメントＶ方向成分への影響を打ち消すための両脚機構変形補償角θｆｆｄｂｖを求める。これは、いわゆるフィードフォワード補償である。
【０１７９】
具体的には、両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖと変形量との関係を表す機構コンプライアンスモデルを用い、目標第１足平床反力中心点Ｑ１と目標第２足平床反力中心点Ｑ２を結ぶ線分の変形角度を求め、それの極性を反転したものを両脚機構変形補償角θｆｆｄｂｖとすれば良い。
【０１８０】
両脚機構変形補償角θｆｆｄｂｖは、近似的には次式により求めれば良い。
【０１８１】
最後に次式によって両脚補償角θｄｂｖを得る。ここでＫｄｂは制御ゲインであり、通常、これは正の値に設定する。
第ｎ足平補償角決定部について説明すると、図２５はその中の第１足平Ｘ補償角決定部の演算処理を示すブロック図であり、第１足平Ｘ補償角θ１ｘは図示の如く演算する。説明は省略するが、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙも同様に求める。ここでは第１足平Ｘ補償角θ１ｘを求めるアルゴリズムだけを説明する。
【０１８３】
第１足平床反力モーメントＸ成分Ｍ１ａｃｔｘをローパスフィルタに通してＭ１ａｃｔｆｉｌｔｘを得る。第１足平補償モーメントＸ成分Ｍｄｍｄ１ｘを補償用フィルタに通し、それを、Ｍ１ａｃｔｆｉｌｔｘから減じ、偏差モーメントＭｄｉｆｆ１ｘを得る。両脚補償角決定と同様、補償用フィルタは、Ｍｄｍｄ１ｘから実全床反力までの伝達関数の周波数応答特性を改善するものである。
【０１８４】
次に、両脚補償角決定と同様、足平ばね機構などの変形による第１足平補償モーメントＸ成分への影響を打ち消すための第１足平Ｘ機構変形補償角θｆｆ１ｘを求める。これは、いわゆるフィードフォワード補償である。
【０１８５】
具体的には、第１足平補償モーメントＸ方向成分Ｍｄｍｄ１ｘと変形量との関係を表す機構コンプライアンスモデルを用い、第１足平の変形角度を求め、それの極性を反転したものを第１足平Ｘ機構変形補償角θｆｆ１ｘとすれば良い。
【０１８６】
第１足平Ｘ機構変形補償角θｆｆ１ｘは、近似的には次式により求めれば良い。
【０１８７】
最後に次式によって第１足平Ｘ補償角θ１ｘを得る。ここでＫ１ｘは制御ゲインであり、通常、これも正の値に設定する。
【０１８８】
図１８に戻って説明を続けると、修正目標足平位置・姿勢算出部は、両脚補償角θｄｂｖ、第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙおよび内力補償変位（後述）に基づき、前述の複合コンプライアンス動作を拡張した足平位置・姿勢修正手法に従って目標足平位置・姿勢を修正し、修正目標足平位置・姿勢を得る。
【０１８９】
機構変形量算出部は、目標各足平床反力によって発生が予想されるばね機構３２や足底弾性体３４の変形量を求める。
【０１９０】
機構変形補償入り修正目標足平位置・姿勢算出部は、算出された機構変形量を打ち消すように、修正目標足平位置・姿勢をさらに修正し、機構変形補償入り修正目標足平位置・姿勢を得る。
【０１９１】
例えば、図２６に示すような機構変形量が予想されるときには、機構変形補償入り修正目標足平位置・姿勢は、図２７に実線で示す位置・姿勢に修正される。即ち、図２７に示す機構変形補償後の足平が目標足平床反力を受けて変形したときの位置・姿勢が、図２６に示す床反力を受けないときの機構変形補償前の足平位置・姿勢に一致するように、機構変形補償入り修正目標足平位置・姿勢を算出する。
【０１９２】
機構変形補償は、ばね機構３２や足底弾性体３４の変形によって生じる実足平位置・姿勢のずれをフィードフォワード的に打ち消す制御であり、この制御がない場合に比較し、より一層、目標歩容に近い歩行を実現することができる。
【０１９３】
上記を前提として図１０フロー・チャートの説明に戻ると、前記の如く、Ｓ３４において上記した補償角を決定する。
【０１９４】
図２８はその作業を示すサブルーチン・フロー・チャートである。
【０１９５】
同図を参照して説明すると、先ずＳ１００において前記したベクトルＶを求め、Ｓ１０２に進んで分配用重み変数を図２０に示すように設定し、現在時刻ｔでのこれらの値を求める。続いてＳ１０４に進み、式３および式４によって補償全床反力モーメントＭｄｍｄを両脚補償モーメントＭｄｍｄｄｂｖと各足平補償モーメントＭｄｍｄｎｘ（ｙ）に分配し、Ｓ１０６に進んで既述の如く両脚補償角θｄｂｖを求め、Ｓ１０８に進んで各足平補償角θｎｘ（ｙ）を求める。
【０１９６】
図１０フロー・チャートの説明に戻ると、次いでＳ３５に進んで各足平内力補償変位を決定するが、これについては後述する。
【０１９７】
次いでＳ３６に進み、目標各足平床反力に基づいて機構変形補償量を算出し、Ｓ３８に進んで目標足平位置・姿勢を補償角θｄｂｖ，θｎｘ（ｙ）、および内力補償変位（後述）に応じて修正し、更にこれを機構変形補償量に応じて修正し、機構変形補償入り修正目標足平位置・姿勢を得る。
【０１９８】
次いでＳ４０に進み、上***置・姿勢と機構変形補償入り修正足平位置・姿勢から関節変位指令（値）を算出し、Ｓ４２に進んで実関節変位を算出された関節変位指令（値）にサーボ制御し、Ｓ４４に進んで時刻をΔｔ更新し、Ｓ１４に戻って上記の処理を繰り返す。
【０１９９】
ここで、図１０フロー・チャートのＳ３５の処理について説明する。
【０２００】
上記したＳ３５を除く処理で行われる制御によってロボット全体の挙動に影響を与える実全床反力のモーメント成分と実各足平モーメントを独立に制御することができる。しかし、実各足平床反力の力成分を独立に制御することはできない。
【０２０１】
本出願人がこの出願と同日付けで提案する、実全床反力の力成分のコンプライアンス制御では、実各足平床反力の力成分の和を制御することができるので、実各足平の力成分を独立に制御するためには、実各足平床反力の力成分の差を制御するものを加えれば良い。Ｓ３５の処理はそれを意図するもので、以下の追加される制御を「内力コンプライアンス制御」という。
【０２０２】
この制御は、目標第１足平と目標第２足平の間隔を僅かに延ばしたり、縮めたりすることによって、両足平を結ぶ線分方向に、足平床反力の力成分を、それぞれの足平に互いに逆向きに発生させる。この動作を「逆位相平行移動動作」という。
【０２０３】
この制御を行うと、実全床反力と実各足平モーメントに対してＳ３５の処理を除く処理で述べた制御に干渉することなく、実第１足平の力成分と実第２足平の力成分の比率を変えることができるので、この比率を状況に応じて適切に制御することによって、前記したように、例えばロボットを進行方向に推進させる力を、接地圧の高い足平には大きくすると共に、低い足平には小さくすることによって、姿勢安定性を向上させてスリップを効果的に防止することができる。
【０２０４】
また、一方の足平が他方の足平の発生する進行方向に推進させる力の負荷となっている状態なども解消させることができ、関節アクチュエータの負担を軽減させることができる。
【０２０５】
その意図から、この実施の形態においては、先に提案した装置に、図１８の下部に示す如く、内力補償変位算出部を追加した。具体的には、内力補償変位算出部および内力補償変位を含む修正目標足平位置姿勢算出部は、Ｓ３５の処理を除く処理で得られた修正目標各足平位置・姿勢に以下の操作を加える。
【０２０６】
図２９は内力補償変位算出部の処理を詳細に示すブロック図であり、図３０はその逆位相平行移動動作の説明図である。図２９に示す如く、内力補償変位算出部は、実各足平床反力の力成分と目標各足平床反力の力成分から、図３０に示す各足平内力補償変位Ｄｎ（ｎ＝１，２）を算出する。また、修正目標足平位置姿勢算出部は、その各足平内力補償変位Ｄｎを含め、幾何学演算によって修正足平位置・姿勢を算出する。
【０２０７】
図３０を参照して概説すると、先ず、始点がＱ１’、始点がＱ２’のベクトルと、向きが同じで大きさが１の方向ベクトルを求める。これを以降Ｖ’と記述する。
【０２０８】
次に、Ｓ３５の処理を除く処理で得られた修正目標第１足平を、姿勢を変えずに、Ｄ１Ｖ’だけ平行移動させる。図示の如く、Ｄ１は修正目標第１足平のＶ’方向への移動量であり、以降、第１足平内力補償変位という。Ｄ１は、スカラ変数である。
【０２０９】
同時に、修正目標第２足平を、姿勢を変えずに、Ｄ２Ｖ’だけ平行移動させる。Ｄ２は修正目標第２足平のＶ’方向への移動量であり、以降、第２足平内力補償変位という。Ｄ１も、スカラ変数である。尚、図示の状態では、Ｄ１は負、Ｄ２は正である。通常、Ｄ１とＤ２は極性が逆であるため、両足平は、逆位相に動く。以上が、逆位相平行移動動作である。
【０２１０】
上記を図３１フロー・チャートを参照してさらに詳細に説明する。
【０２１１】
Ｓ２００において図３２の時変特性を示す、予め設定された分配用重み変数Ｗｄ１，Ｗｄ２の現在時刻ｔにおける値を求める。
【０２１２】
続いてＳ２０２に進み、実各足平床反力の力成分および目標各足平床反力の力成分から式１９によって偏差Ｆｄｅｒｒを算出する。即ち、図２９に示すように、実第１足平床反力の力成分Ｆ１ａｃｔ、実第２足平床反力の力成分Ｆ２ａｃｔ、目標第１足平床反力の力成分Ｆ１ｒｅｆ、および目標第２足平床反力の力成分Ｆ２ｒｅｆから、次の式（式１９）によって偏差Ｆｄｅｒｒを算出する。
【０２１３】
Ｆｄｅｒｒ＝Ｆ２ａｃｔ−Ｆ１ａｃｔ−（Ｆ２ｒｅｆ−Ｆ１ｒｅｆ）・・・式１９
尚、目標第１足平床反力の力成分Ｆ１ｒｅｆおよび目標第２足平床反力の力成分Ｆ２ｒｅｆは、前記した目標歩容の中の値である。
【０２１４】
続いてＳ２０４に進み、算出した偏差Ｆｄｅｒｒを制御系の発振を防止するためのローパスフィルタに通し、さらに、ゲインＫｄ倍して差Ｄｉｆｆを算出する。
【０２１５】
続いてＳ２０６に進み、次式（式２０，２１）によって、第１足平内力補償変位Ｄ１と第２足平内力補償変位Ｄ２を算出する。
Ｄ１＝−Ｗｄ１＊Ｄｄｉｆｆ ．．．式２０
Ｄ２＝ Ｗｄ２＊Ｄｄｉｆｆ ．．．式２１
【０２１６】
ここで、Ｗｄ１およびＷｄ２は、非負の分配重み変数である。Ｗｄ１とＷｄ２の和は１のとき、Ｄｄｉｆｆは、修正目標第２足平と修正目標第１足平の間の距離の伸び量を意味する。
【０２１７】
分配用重み変数Ｗｄｎの設定例は図３２に示す通りであるが、図示の例では以下の点を考慮して設定される。即ち、各足平内力補償変位が不連続に変化すると、関節に過大なトルクが発生することから、Ｗｄｎは、連続的に変化するように設定される。
【０２１８】
また、足平が着地する頃にＷｄｎが零でないと、足平着地位置が目標位置からずれ、却ってこの内力制御の支障となる場合がある。従って、足平が着地する付近ではＷｄｎは零にする。また、片足支持期は制御不要であることから、Ｗｄｎを零にすると共に、両脚支持期および直立時にはＷｄ１とＷｄ２の和を１にする。
【０２１９】
かく得られた同位相補償変位Ｄｎも含め、図１０フロー・チャートのＳ３８において機構変形補償入り修正目標足平位置姿勢が算出される。
【０２２０】
具体的には、両脚補償角θｄｂｖ、第１足平Ｘ補償角θ１ｘ、第１足平Ｙ補償角θ１ｙ、第２足平Ｘ補償角θ２ｘ、第２足平Ｙ補償角θ２ｙ、第１足平内力補償変位Ｄ１、および第２足平内力補償変位Ｄ２に基づき、前記した同位相平行移動動作を含む複合コンプライアンス動作に従って修正目標足平位置姿勢が算出される。
【０２２１】
この実施の形態では上記の如く、少なくとも基体（上体２４）と、前記基体に第１の関節（１０，１２，１４Ｒ（Ｌ））を介して連結されると共に、その先端に第２の関節（１８，２０Ｒ（Ｌ））を介して連結される足部（足平２２Ｒ（Ｌ））を備えた複数本（２本）の脚部（脚部リンク２）からなる脚式移動ロボットの制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上***置・姿勢、目標足平位置・姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（＝目標ＺＭＰ））を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（歩容生成器、Ｓ１０からＳ２２）、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、Ｓ２４，Ｓ２６）、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（６軸力センサ４４、実各足平床反力検出器、Ｓ３２）、前記検出された実床反力が前記算出された目標足部床反力中心点まわりに作用するモーメント（実第ｎ足平床反力モーメントＭａｃｔｘ，ｙ，ｚ）を算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる回転量（両脚補償角θｄｂｖ，ｚ、第ｎ足平補償角θｎｘ，ｙ，ｚ）を決定する足部回転量決定手段（複合コンプライアンス動作決定部、Ｓ３２からＳ３４、両脚補償角決定部、第ｎ足平補償角決定部、Ｓ１００からＳ１０８）、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が回転するように前記目標位置および／または姿勢を修正する第１の足部位置・姿勢修正手段（複合コンプライアンス動作決定部、Ｓ３８，Ｓ４０、修正目標足平位置・姿勢算出部）、前記検出された実床反力から実全床反力に影響しない内力成分（Ｆ１ａｃｔ＋Ｆ２ａｃｔ）を求めて前記生成された歩容の内力成分（Ｆ１ｒｅｆ＋Ｆ２ｒｅｆ）との偏差Ｆｄｅｒｒ，Ｄｄｉｆｆを算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位Ｄｎを算出する補償変位算出手段（内力補償変位算出部、Ｓ３５，Ｓ２００からＳ２０６）、前記算出された補償変位に基づいて目標足部位置・姿勢を修正する第２の足部位置・姿勢修正手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３８）および前記第１および前記第１および第２の足部位置・姿勢手段によって修正された足部の位置・姿勢に基づいて前記ロボットの第１および第２の関節（１０，１２，１４，１８，２０Ｒ（Ｌ））を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、Ｓ４０，Ｓ４２）を備える如く構成した。
【０２２２】
また、少なくとも基体（上体２４）と、前記基体に第１の関節（１０，１２，１４Ｒ（Ｌ））を介して連結されると共に、その先端に第２の関節（１８，２０Ｒ（Ｌ））を介して連結される足部（足平２２Ｒ（Ｌ））を備えた複数本（２本）の脚部（脚部リンク２）からなる脚式移動ロボット１の制御装置において、前記ロボットの少なくとも前記足部の目標位置および姿勢を含む運動パターン（目標上***置・姿勢、目標足平位置・姿勢）と、前記ロボットに作用する全床反力の目標パターン（目標全床反力、目標全床反力中心点（＝目標ＺＭＰ））を少なくとも含む前記ロボットの歩容を生成する歩容生成手段（歩容生成器、Ｓ１０からＳ２２）、前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点（目標各足平床反力中心点）を決定する目標足部床反力中心点決定手段（目標床反力分配器、Ｓ２４）、前記足部に作用する実床反力（実各足平床反力）を検出する実床反力検出手段（６軸力センサ４４、実各足平床反力検出器、Ｓ３２）、少なくとも前記検出された実床反力に基づいて前記足部を回転させる回転量（両脚補償角θｄｂｖ，ｚ、第ｎ足平補償角θｎｘ，ｙ，ｚ）を決定する足部回転量決定手段（複合コンプライアンス動作決定部、Ｓ３２，Ｓ３４、両脚補償角決定部、第ｎ足平補償角決定部、Ｓ１００からＳ１０８）、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および／または姿勢を修正する第１の足部位置・姿勢修正手段（複合コンプライアンス動作決定部、Ｓ３８，Ｓ４０、修正目標足平位置・姿勢算出部）、前記検出された実床反力から実全床反力に影響しない内力成分（Ｆ１ａｃｔ＋Ｆ２ａｃｔ）を求めて前記生成された歩容の内力成分（Ｆｒｅｆ１＋Ｆｒｅｆ２）との偏差Ｆｄｅｒｒ，Ｄｄｉｆｆを算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位Ｄｎを算出する補償変位算出手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３５，Ｓ２００からＳ２０６）、前記算出された補償変位に基づいて目標足部位置・姿勢を修正する第２の足部位置・姿勢修正手段（上体Ｚ方向加速度制御演算部、同位相補償変位算出部、Ｓ３８）および前記第１および前記第１および第２の足部位置・姿勢手段によって修正された足部の位置・姿勢に基づいて前記ロボットの第１および第２の関節（１０，１２，１４，１８，２０Ｒ（Ｌ））を変位させる関節変位手段（ロボット幾何学モデル（キネマティクス演算部）、変位コントローラ、Ｓ４２）を備えるように構成した。
【０２２３】
また、前記第１の足部位置・姿勢修正手段は、前記決定された足部回転量に基づいて前記足部の位置および／または姿勢が、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転するように、前記目標位置および／または姿勢を修正する如く構成した。
【０２２４】
さらに、前記ロボットに実際に作用する全床反力モーメント（より正確にはモーメント成分ＰＱ１ ＊Ｆ１ａｃｔ ＋ＰＱ２ ＊Ｆ２ａｃｔ ＋Ｍ１ａｃｔ ＋Ｍ２ａｃｔ）、または前記ロボットに実際に作用する全床反力のモーメント（ＰＱ１ ＊Ｆ１ａｃｔ ＋ＰＱ２ ＊Ｆ２ａｃｔ ＋Ｍ１ａｃｔ ＋Ｍ２ａｃｔ ）から前記足部に作用する床反力モーメント（Ｍ１ａｃｔ ＋Ｍ２ａｃｔ）を減算して得たモーメント（Ｍｆ１ｆ２ａｃｔ＝ＰＱ１ ＊Ｆ１ａｃｔ ＋ＰＱ２ ＊Ｆ２ａｃｔ ）のいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる移動量（θｄｂｖ，ｚ）を決定する足部移動量決定手段（複合コンプライアンス動作決定部、Ｓ３４、両脚補償角決定部、Ｓ１００からＳ１０８）を備え、前記第１の足部位置・姿勢修正手段は、前記決定された足部回転量および前記決定された移動量に基づいて前記足部の位置および／または姿勢を修正するように構成した。
【０２２５】
また、前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モーメント（補償全床反力Ｍｄｍｄ）を求め、前記足部回転量決定手段および／または前記足部移動量決定手段は、少なくとも前記検出された実床反力（実各足平床反力）と前記求めた姿勢安定化補償全床反力モーメントに基づいて前記足部の回転量および／または移動量を決定する（Ｓ３４，Ｓ１００からＳ１０８）如く構成した。
【０２２６】
また、前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差（θｅｒｒｘ，ｙ）に基づいて求める（Ｓ２８，Ｓ３０）如く構成した。
【０２２７】
また、前記補償変位算出手段は、前記偏差（Ｆｄｅｒｒ，Ｄｄｉｆｆ）に所定の重み（Ｗｄｎ）を乗じて前記足部の補償変位を足部のそれぞれに分配する如く構成した。
【０２２８】
また、前記所定の重みが時変特性および／または周波数特性を持つ如く構成した。
【０２２９】
上記の如く構成したので、これによって、概括すれば、実全床反力の制御と実各足平床反力の制御が殆ど干渉しないようになり、それらを容易に制御することができる。
【０２３０】
即ち、この実施の形態に係る装置は、先に提案した技術に対して以下の点を改良した。即ち、特開平５−３０５５８４号公報で提案した足首コンプライアンス制御では、足首または足底の基準点などの足平に固定された点における実床反力モーメントを検出し、それに基づいて前記固定された点を中心に足平を回転させていたが、この実施の形態に係る装置では、移動する目標足平床反力中心点における実各足平床反力モーメントを算出し、それに基づいて目標足平床反力中心点を中心に足平を回転させるように変更し、その点まわりのモーメントを望ましい値に制御するようにした。
【０２３１】
この結果、実全床反力と実各足平床反力がほとんど干渉することなく、容易に制御することが可能となった。より干渉を少なくするために、各瞬間における想定していた足底接地領域内にもっと適切な点を選定しても良い。
【０２３２】
従って、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。
【０２３３】
また、ロボットの実各足平床反力の力成分を独立に制御することで姿勢安定性を向上させてスリップを効果的に防止することができると共に、関節アクチュエータの負担を軽減させることができる。
【０２３４】
また、上記した実施の形態において、ばね機構３２（および足底弾性体３４）自身はこの発明の本質部分ではない。この発明の本質はフィードバック制御部分にあり、機構変形補償は付随的なものである。
【０２３５】
また、上記した実施の形態において、ブロック線図は演算処理順序を変えるなどの等価変形をしても良い。
【０２３６】
また、上記した実施の形態を、先に提案した出願の第１の実施の形態に関して説明したが、第２あるいはそれ以降の実施の形態に関して追加しても良い。
【０２３７】
また、この発明を２足歩行ロボットに関して説明してきたが、２足歩行ロボットに限らず、多脚ロボットにも応用することができる。
【０２３８】
【発明の効果】
請求項１項にあっては、脚式移動ロボットに作用する床反力を、干渉を生じることなく、容易かつ適切に制御することができる。換言すれば、先に提案した両脚コンプライアンス制御および足首コンプライアンス制御の併用に近い制御を行っても、制御の干渉がなく、実全床反力と実各足平床反力が望ましい値からずれたり発振することがない。
【０２３９】
また、ロボットの実各足平床反力が作り出す内力成分（実全床反力に影響しない成分の組み合わせ）を独立に制御することで姿勢安定性を向上させてスリップを効果的に防止することができると共に、関節アクチュエータの負担を軽減させることができる。
【０２４０】
さらには、大域的なうねりや傾斜だけでなく、局所的な凹凸や傾斜なども含む予期しない床形状変化があっても、その影響をあまり受けずに脚式移動ロボットに作用する床反力を適切に制御することができる。
【０２４１】
請求項２項にあっては、請求項１項と同様の作用効果を得ることができる。
【０２４２】
請求項３項にあっては、請求項１項と同様の作用効果を得ることができると共に、床反力をより適切に制御することができる。
【０２４３】
請求項４項にあっては、請求項１項と同様の作用効果を得ることができると共に、特に姿勢制御に重要な全床反力を一層適切に制御することができる。
【０２４４】
請求項５項にあっては、請求項１項と同様の作用効果を得ることができると共に、姿勢安定化能力を向上させることができる。
【０２４５】
請求項６項にあっては、前記したと同様の作用効果を得ることができると共に、姿勢安定化能力を一層向上させることができる。
【０２４６】
請求項７項にあっては、前記したと同様の作用効果を得ることができる。
【０２４７】
請求項８項にあっては、前記したと同様の作用効果を得ることができると共に、補償変位の分配を一層適正に行うことができる。
【図面の簡単な説明】
【図１】この発明に係る脚式移動ロボットの制御装置を全体的に示す説明図である。
【図２】図１に示す２足歩行ロボットの足部の構造を示す説明側面図である。
【図３】図１に示す２足歩行ロボットの制御ユニットの詳細を示すブロック図である。
【図４】この発明に係る脚式移動ロボットの制御装置の構成および動作を機能的に示すブロック図である。
【図５】図１に示す脚式移動ロボットが平地を歩行するときの運動パターンの一例を示す説明図である。
【図６】図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡の床面上軌跡を示す説明図である。
【図７】図５の運動パターンに対応する目標全床反力中心点（目標ＺＭＰ）軌跡のタイム・チャートである。
【図８】図５の運動パターンに対応する所定の条件を満たすように設定した目標第１足平床反力中心点軌跡のタイム・チャートである。
【図９】図５の運動パターンに対応する所定の条件を満たすように設定した目標第２足平床反力中心点軌跡のタイム・チャートである。
【図１０】図４と同様に、この発明に係る脚式移動ロボットの制御装置の動作を示すフロー・チャートである。
【図１１】図１０フロー・チャートの内の両脚補償角などの演算処理を行う、図４に示す複合コンプライアンス動作決定部の動作を説明するための、両脚支持期に第１足平と第２足平に実各足平床反力が作用している状況を示す説明図である。
【図１２】図１１に示す状況における目標全床反力の設定を示す説明図である。
【図１３】図１１に示す状況における目標各足平床反力の分配を示す説明図である。
【図１４】図１１に示す状況における補償全床反力モーメントを示す説明図である。
【図１５】図１１に示す状況における、各足平床反力中心点を含み、水平面に垂直な平面の法線ベクトルＶを示す説明図である。
【図１６】図１１に示す状況における、目標各足平床反力中心点を目標全床反力中心点（目標ＺＭＰ）まわりに、所定角度θｄｂｖだけ回転させたときの状態を示す説明図である。
【図１７】図１６に示す状況における、各足平を前後方向軸および左右方向軸まわりに所定角度θｎｘ，θｎｙだけ回転させたときの状態を示す説明図である。
【図１８】図４の複合コンプライアンス動作決定部の演算処理を示すブロック図である。
【図１９】図１８に示す補償全床反力モーメント分配器の演算処理を示すブロック図である。
【図２０】図１８に示す補償全床反力モーメント分配器の、両脚補償角などを操作するための分配重み変数の設定例を示すタイム・チャートである。
【図２１】図２０の補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。
【図２２】図２１と同様に、補償全床反力モーメント分配器の分配重み変数の設定を説明するための、ロボットの姿勢を示す説明図である。
【図２３】両脚補償角を操作するための分配重みを所定の条件で決定したときの両脚補償モーメントＶ方向成分Ｍｄｍｄｄｂｖを示す説明図である。
【図２４】図１８に示す両脚補償角決定部の演算処理を示すブロック図である。
【図２５】図１８に示す各足平の補償角決定部の演算処理を示すブロック図である。
【図２６】図１８に示す機構変形補償入り修正目標足平位置・姿勢算出部の演算処理を説明するための説明図である。
【図２７】図２６と同様に、図１８に示す機構変形補償入り修正目標足平位置・姿勢算出部の演算処理を説明するための説明図である。
【図２８】図１０フロー・チャートの内の両脚補償角などの決定作業を示すサブルーチン・フロー・チャートである。
【図２９】図１８の内力補償変位算出部の処理を詳細に示すブロック図である。
【図３０】図２９ブロック図の処理を説明する、図１６と同様な説明図である。
【図３１】図１８の内力補償変位算出部の処理を示すフロー・チャートである。
【図３２】図２９および図３１で使用する重みの設定例を示す説明図である。
【図３３】２足歩行ロボットが予期しなかった傾斜面を歩行するときの説明図である。
【図３４】図３３に示す２足歩行ロボットに対して先に提案した両脚コンプライアンス制御を行った場合の説明図である。
【図３５】図３３に類似する、２足歩行ロボットが予期しなかった突起を踏んだときの説明図である。
【図３６】図３５に示す状況で、先に提案した足首コンプライアンス制御を行ったときの説明図である。
【符号の説明】
１ ２足歩行ロボット（脚式移動ロボット）
２ 脚部リンク
１０，１２，１４Ｒ，Ｌ 股関節
１６Ｒ，Ｌ 膝関節
１８，２０Ｒ，Ｌ 足関節
２２Ｒ，Ｌ 足平（足部）
２４ 上体
２６ 制御ユニット
３２ ばね機構
３４ 足底弾性体
４４ ６軸力センサ
６０ 傾斜センサ
Claims (8)
- 少なくとも基体と、前記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、
ａ．前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、
ｂ．前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、
ｃ．前記足部に作用する実床反力を検出する実床反力検出手段、
ｄ．前記決定された目標足部床反力中心点まわりに前記検出された実床反力が作用するモーメントを算出し、少なくとも前記算出されたモーメントに基づいて前記足部を回転させる足部回転量を決定する足部回転量決定手段、
ｅ．前記検出された実床反力から実全床反力に影響しない内力成分を求めて前記生成された歩容の内力成分との偏差を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位を決定する補償変位決定手段、
ｆ．前記決定された足部回転量に基づいて前記足部の位置または姿勢を回転させつつ、前記決定された補償変位に基づいて前記足部の目標位置または姿勢を移動させることによって、前記足部の目標位置または姿勢を修正する足部位置・姿勢修正手段、
および
ｇ．前記足部位置・姿勢修正手段によって修正された前記足部の目標位置・姿勢に基づいて前記第１および第２の関節を変位させる関節変位手段、
を備えたことを特徴とする脚式移動ロボットの制御装置。 - 少なくとも基体と、前記基体に第１の関節を介して連結されると共に、その先端に第２の関節を介して連結される足部を備えた複数本の脚部からなる脚式移動ロボットの制御装置において、
ａ．前記ロボットの少なくとも前記足部の目標位置または姿勢を含む運動パターンと、前記ロボットに作用する全床反力の目標パターンを少なくとも含む前記ロボットの歩容を生成する歩容生成手段、
ｂ．前記生成された歩容の全床反力を前記足部のそれぞれに分配したときの前記足部上の作用中心点たる目標足部床反力中心点を決定する目標足部床反力中心点決定手段、
ｃ．前記足部に作用する実床反力を検出する実床反力検出手段、
ｄ．少なくとも前記検出された実床反力に基づいて前記足部を回転させる足部回転量を決定する足部回転量決定手段、
ｅ．前記検出された実床反力から実全床反力に影響しない内力成分を求めて前記生成された歩容の内力成分との偏差を算出し、前記算出した偏差に基づいて前記足部のそれぞれの補償変位を決定する補償変位決定手段、
ｆ．前記決定された足部回転量に基づいて前記足部の位置または姿勢を、前記決定された目標足部床反力中心点あるいはその近傍まわりに回転させつつ、前記決定された補償変位に基づいて前期足部の目標位置または姿勢を移動させることによって、前記足部の目標位置または姿勢を修正する第２の足部位置・姿勢修正手段、
および
ｇ．前記足部位置・姿勢修正手段によって修正された前記足部の位置または姿勢に基づいて前記第１および第２の関節を変位させる関節変位手段、
を備えたことを特徴とする脚式移動ロボットの制御装置。 - 前記足部位置・姿勢修正手段は、さらに前記決定された足部回転量に基づいて前記足部の位置または姿勢が、前記決定された目標足部床反力中心点またはその近傍まわりに回転するように、前記足部の目標位置または姿勢を修正することを特徴とする請求項１項記載の脚式移動ロボットの制御装置。
- さらに、
ｈ．前記ロボットに実際に作用する全床反力モーメント、または前記ロボットに実際に作用する全床反力モーメントから前記足部に作用する床反力モーメントを減算して得たモーメントのいずれかを算出し、少なくとも前記算出されたモーメントに応じて前記足部を移動させる足部移動量を決定する足部移動量決定手段、
を備え、前記足部位置・姿勢修正手段は、前記決定された足部回転量、前記決定された補償変位および前記決定された足部移動量に基づいて前記足部の目標位置または姿勢を修正することを特徴とする請求項１項から３項のいずれかに記載の脚式移動ロボットの制御装置。 - 前記全床反力の目標パターンに付加する姿勢安定化補償全床反力モーメントを求め、前記足部回転量決定手段または前記足部移動量決定手段は、少なくとも前記検出された実床反力と前記求めた姿勢安定化補償全床反力モーメントに基づいて前記足部回転量または足部移動量を決定することを特徴とする請求項１項から４項のいずれかに記載の脚式移動ロボットの制御装置。
- 前記姿勢安定化補償全床反力モーメントを、少なくとも前記ロボットの傾き偏差に基づいて求めることを特徴とする請求項５項記載の脚式移動ロボットの制御装置。
- 前記補償変位決定手段は、前記偏差に所定の重みを乗じて前記足部の補償変位を足部のそれぞれに分配することを特徴とする請求項１項から６項記載の脚式移動ロボットの制御装置。
- 前記所定の重みが時変特性および周波数特性の少なくともいずれかを持つことを特徴とする請求項７項記載の脚式移動ロボットの制御装置。
Priority Applications (5)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP12523398A JP3629143B2 (ja) | 1998-04-20 | 1998-04-20 | 脚式移動ロボットの制御装置 |
| US09/673,629 US6289265B1 (en) | 1998-04-20 | 1999-04-19 | Controller for legged mobile robot |
| EP99913713A EP1120203B1 (en) | 1998-04-20 | 1999-04-19 | Controller for legged mobile robot |
| PCT/JP1999/002075 WO1999054095A1 (fr) | 1998-04-20 | 1999-04-19 | Controleur pour robot mobile muni de jambes |
| DE69943148T DE69943148D1 (de) | 1998-04-20 | 1999-04-19 | Steuereinheit für einen mit beinen beweglichen roboter |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP12523398A JP3629143B2 (ja) | 1998-04-20 | 1998-04-20 | 脚式移動ロボットの制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH11300661A JPH11300661A (ja) | 1999-11-02 |
| JP3629143B2 true JP3629143B2 (ja) | 2005-03-16 |
Family
ID=14905115
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP12523398A Expired - Fee Related JP3629143B2 (ja) | 1998-04-20 | 1998-04-20 | 脚式移動ロボットの制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3629143B2 (ja) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4611580B2 (ja) * | 2001-06-27 | 2011-01-12 | 本田技研工業株式会社 | トルク付与システム |
| JP4225968B2 (ja) | 2002-04-26 | 2009-02-18 | 本田技研工業株式会社 | 脚式移動ロボットの制御装置 |
| JP2004181599A (ja) * | 2002-12-05 | 2004-07-02 | Sony Corp | 脚式移動ロボット |
| JP4086765B2 (ja) * | 2003-11-28 | 2008-05-14 | ソニー株式会社 | ロボット装置 |
| US7366587B2 (en) | 2002-12-05 | 2008-04-29 | Sony Corporation | Legged mobile robot |
| JP2004181600A (ja) * | 2002-12-05 | 2004-07-02 | Sony Corp | 脚式移動ロボット |
| JP2004181601A (ja) * | 2002-12-05 | 2004-07-02 | Sony Corp | 脚式移動ロボット |
| US7379789B2 (en) | 2003-06-27 | 2008-05-27 | Honda Motor Co., Ltd. | Gait generating device of legged mobile robot and legged mobile robot controller |
| EP1649983B1 (en) | 2003-06-27 | 2010-09-22 | Honda Motor Co., Ltd. | Gait generation device for legged mobile robot |
| JP5268107B2 (ja) * | 2009-03-16 | 2013-08-21 | 学校法人立命館 | 多脚歩行式移動装置のハイブリッド制御装置及び手法 |
| JP5616290B2 (ja) * | 2011-05-25 | 2014-10-29 | 本田技研工業株式会社 | 移動体の制御装置 |
| JP5616288B2 (ja) * | 2011-05-25 | 2014-10-29 | 本田技研工業株式会社 | 移動体の制御装置 |
| JP5979049B2 (ja) * | 2013-03-22 | 2016-08-24 | トヨタ自動車株式会社 | 脚式移動ロボットおよびその制御方法 |
-
1998
- 1998-04-20 JP JP12523398A patent/JP3629143B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH11300661A (ja) | 1999-11-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3629133B2 (ja) | 脚式移動ロボットの制御装置 | |
| EP1120203B1 (en) | Controller for legged mobile robot | |
| JP3634238B2 (ja) | 脚式移動ロボットの床形状推定装置 | |
| EP1514777B1 (en) | Control system of legged mobile robot | |
| JP3726009B2 (ja) | 脚式移動ロボットの床形状推定装置 | |
| US5355064A (en) | Control system for legged mobile robot | |
| JP4912891B2 (ja) | 脚式移動ロボットおよびその制御プログラム | |
| JP5053644B2 (ja) | 脚式移動ロボットおよびその制御プログラム | |
| JP3148827B2 (ja) | 脚式移動ロボットの歩行制御装置 | |
| JP3629143B2 (ja) | 脚式移動ロボットの制御装置 | |
| JP5123527B2 (ja) | 脚式移動ロボットおよびその制御プログラム | |
| JP3270766B2 (ja) | 脚式移動ロボットの制御装置 | |
| JPH05305579A (ja) | 脚式移動ロボットの歩行制御装置 | |
| JPH05305583A (ja) | 脚式移動ロボットの歩行制御装置 | |
| JP3629142B2 (ja) | 脚式移動ロボットの制御装置 | |
| JPH05305586A (ja) | 脚式移動ロボットの歩行制御装置 | |
| JP3405868B2 (ja) | 脚式歩行ロボットの歩容生成方法 | |
| JP4237130B2 (ja) | 脚式移動ロボットの制御装置 | |
| JP3726097B2 (ja) | 脚式移動ロボットの姿勢制御装置 | |
| JP3183557B2 (ja) | 脚式移動ロボットの歩行制御装置 | |
| JP2997037B2 (ja) | 脚式移動ロボットの歩行制御装置 | |
| JPH06182679A (ja) | リンク式ロボットの制御装置 |
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 |
|
| LAPS | Cancellation because of no payment of annual fees |