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

Description

【０００１】
【発明の属する技術分野】
この発明は、脚式移動ロボットの姿勢制御装置に関し、より詳しくは脚式移動ロボット、特に２足歩行の脚式移動ロボットにおいて予期せぬ対象物反力を受けても動的バランスをとって姿勢の安定性を保つことができるようにしたものに関し、特に腕を備える脚式移動ロボットの腕と脚の脚腕協調制御としての応用が有効である。
【０００２】
尚、この明細書で『対象物反力』は作業対象を含む環境から受ける外力で、ロボットに接地面から作用する床反力を除いたものを指称する意味で使用する。
【０００３】
【従来の技術】
脚式移動ロボット、特に２足歩行の脚式移動ロボットで腕を備えたものとしては、「上体運動により３軸モーメントを補償する２足歩行ロボットの開発」（日本ロボット学会誌１１巻第４号、１９９３年５月）が知られている。このロボットは単純化された腕としての振り子を備え、これを振ることによって発生する重力と慣性力も含めて目標歩容を予め設計しておき、それに追従するように歩行制御する。
【０００４】
【発明が解決しようとする課題】
しかしながら、この従来技術においては腕は対象物反力を受けないことが前提となっている。従って、そこで提案されている制御を歩行だけではなく、作業にも適用したとき、作業対象から予期せぬ反作用を受けると動バランスを崩し、姿勢が不安定になったり、最悪の場合には転倒する恐れがあった。
【０００５】
また、本出願人も特開平７−２０５０６９号公報で同種の脚式移動ロボットを提案しており、そこにおいては歩行時に摩擦力が低下したとき腕を振らせて安定な姿勢を回復するようにしている。
【０００６】
しかしながら、本出願人が提案した脚式移動ロボットにおいては、脚と腕が協調せずに独立して別々に制御されているため、腕を駆動すると、腕が発生する重力と慣性力、および作業対象からの反作用によってロボット全体の動バランスが崩れ、却ってロボットの姿勢が不安定になる場合があった。
【０００７】
従って、この発明の目的は上記した不都合を解消することにあり、脚式移動ロボットが予期できない対象物反力を受けても、動バランスをとって安定な姿勢を継続できるようにした脚式移動ロボットの姿勢制御装置を提供することにある。
【０００８】
この発明の第２の目的は、対象物反力が急変するときも、それに静的にバランスする位置にロボットの重心を移動させることにより、傾きや転倒を効果的に抑制するようにした脚式移動ロボットの姿勢制御装置を提供することにある。
【０００９】
この発明の第３の目的は、上記した対象物反力を受けたとき、ロボットの重心が移動する過渡期においても、重心位置や床反力を適正に変化させて動バランスを維持し続けられるようにした脚式移動ロボットの姿勢制御装置を提供することにある。
【００１０】
この発明の第４の目的は、脚式移動ロボットで腕を備えるものにおいて、予め想定していなかった動作パターンで腕を動かして作業をするときに腕に発生する重力、慣性力だけでなく、作業対象から予期せぬ反作用を受けても、動バランスをとって安定な姿勢を継続できるようにした脚式移動ロボットの姿勢制御装置を提供することにある。
【００１１】
【課題を解決するための手段】
上記の目的を達成するために、請求項１項にあっては、少なくとも基体と、前記基体に連結される複数本のリンクからなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標軌跡を含む運動パターンと、前記ロボットに作用する床反力の目標軌跡と、前記ロボットに作用する床反力以外の外力の目標軌跡とを少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段、前記床反力以外の外力を検出する外力検出手段、前記検出された外力と、前記目標軌跡で設定された床反力以外の外力との偏差を演算する外力偏差演算手段、前記床反力の摂動と前記ロボットの重心位置および基体の位置の少なくともいずれかの摂動の関係を表現するモデル、少なくとも前記演算された外力の偏差に基づいて前記モデルに入力すべきモデル入力量を演算するモデル入力量演算手段、前記演算されたモデル入力量を前記モデルに入力し、得られる前記重心位置および基体の少なくともいずれかの摂動量に応じて前記基体の目標軌跡を修正する、基体目標軌跡修正量を演算する基体目標軌跡修正量演算手段、少なくとも前記演算されたモデル入力量に応じて前記床反力の目標軌跡を修正する、床反力目標軌跡修正量を演算する床反力目標軌跡修正量演算手段、および、少なくとも前記演算された基体目標軌跡修正量および床反力目標軌跡修正量に基づいて前記ロボットの関節を変位させる関節変位手段、を備える如く構成した。
【００１２】
ここで『位置』は、重心位置を除き、『位置および／または姿勢』を含む意味で使用する。尚、『姿勢』は後述の如く３次元空間における向きを意味する。
【００１３】
ここで、『床反力の目標軌跡』とは、より具体的には、少なくとも床反力中心点の目標軌跡を含む意味で使用する。また『前記床反力の目標軌跡を修正する』とはより具体的には、床反力中心点まわりのモーメントを修正する意味で使用する。
【００１４】
ここで、『外力を検出する』とは、検出のみならず外乱オブザーバなどを使用して推定することも含む意味で使用する。
【００１５】
請求項２項にあっては、前記モデル入力量演算手段は、前記外力に静的に平衡する平衡重心位置の摂動量を算出する平衡重心位置摂動量算出手段、を備え、前記算出された平衡重心位置に前記モデルが収束するように前記モデル入力量を演算する如く構成した。
【００１６】
請求項３項にあっては、前記モデルが前記ロボットを倒立振子で近似するモデルである如く構成した。
【００１７】
請求項４項にあっては、前記平衡重心位置摂動量算出手段は、前記算出された平衡重心位置の摂動量を所定の範囲に制限するリミッタ、を備える如く構成した。
【００１８】
請求項５項にあっては、前記床反力目標軌跡修正量演算手段は、前記演算された床反力目標軌跡修正量を所定の範囲に制限するリミッタ、を備える如く構成した。
【００１９】
請求項６項にあっては、前記床反力の目標軌跡は、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む如く構成した。
【００２０】
請求項７項にあっては、前記床反力目標軌跡修正量演算手段は、前記床反力目標軌跡修正量が、前記モデル入力量から前記外力の偏差を減算した値と、前記床反力の目標中心点まわりに作用するモーメントに動力学的に釣り合うように、前記床反力目標軌跡修正量を演算する如く構成した。
【００２１】
請求項８項にあっては、前記床反力以外の外力が、前記リンクを介して前記ロボットに作用する作業対象物からの反力である如く構成した。
【００２２】
請求項９項にあっては、前記ロボットが、前記基体に連結される２本の脚リンクと２本の腕リンクからなる脚式移動ロボットである如く構成した。
【００２３】
請求項１０項にあっては、少なくとも基体と、前記基体に連結される複数本のリンクからなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンと、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段、前記検出された対象物反力を前記目標中心点まわりのモーメントとして変換する対象物反力モーメント変換手段、前記変換された対象物反力モーメントに動力学的に釣り合うように、前記目標中心点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段、および前記修正された目標中心点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段、を備える如く構成した。
【００２４】
請求項１１項にあっては、少なくとも基体と、前記基体に連結される複数本のリンクからなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンを設定する目標歩容設定手段、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段、前記検出された対象物反力を所定の点まわりのモーメントとして変換する対象物反力モーメント変換手段、前記変換された対象物反力モーメントに動力学的につりあうように、前記所定の点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段、および前記修正された所定の点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段、を備える如く構成した。
【００２５】
上記で、『脚式移動ロボット』は腕以外に対象物反力を受ける脚式移動ロボットを含む。また、『腕リンク』に関しては、脚リンクであっても、それが作業対象物に作用するものであれば、腕リンクとみなすものとする。例えば、昆虫型の６脚ロボットにおいて、前の２脚を用いて物を持ち上げる場合には、その脚リンクは腕リンクとみなすこととする。
【００２６】
【作用】
請求項１項においては、脚式移動ロボットが予期できない外力、より具体的には作業対象物から反力を受けても、動バランスをとって安定な姿勢を継続することができる。更に、予め想定していなかった運動パターンでリンク、より具体的には腕を動かして作業をするときに腕に発生する重力、慣性力だけでなく、作業対象から予期せぬ反作用を受けても、動バランスをとって安定な姿勢を継続することができる。
【００２７】
また、対象物反力が急変するときも、それに静的にバランスをとる位置にロボットの重心を移動させることにより、傾きや転倒を効果的に抑制することができる。また、ロボットの重心が移動する過渡期においても、重心位置や床反力を適正に変化させて動バランスを維持し続けることができる。
【００２８】
請求項２項ないし８項においても、請求項１項と同様の作用、効果を有する。
【００２９】
請求項９項にあっては、上記した作用、効果に加えて、脚式移動ロボットで腕を備えるものにおいても、予め想定していなかった動作パターンで腕を動かして作業をするときに腕に発生する重力、慣性力だけでなく、作業対象から予期せぬ反作用を受けても、動バランスをとって安定な姿勢を継続することができる。
【００３０】
請求項１０項ないし１１項においても、請求項１項と同様の作用、効果を有する。
【発明の実施の形態】
以下、添付図面を参照してこの発明に係る脚式移動ロボットの姿勢制御装置を説明する。尚、脚式移動ロボットとしては２足歩行ロボットを例にとる。
【００３１】
図１はその脚式移動ロボットの姿勢制御装置を全体的に示す概略図である。
【００３２】
図示の如く、２足歩行ロボット１は左右それぞれの脚リンク２に６個の関節を備える（理解の便宜のために各関節をそれを駆動する電動モータで示す）。
【００３３】
６個の関節は上から順に、腰部の脚回旋用の関節１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）、腰部のロール軸（Ｙ軸まわり）の関節１２Ｒ，１２Ｌ、同ピッチ軸（Ｘ軸まわり）の関節１４Ｒ，１４Ｌ、膝部のロール軸の関節１６Ｒ，１６Ｌ、足部のロール方向の関節１８Ｒ，１８Ｌ、および同ピッチ軸の関節２０Ｒ，２０Ｌから構成される。足部には足平２２Ｒ，２２Ｌが取着される。
【００３４】
上記において股関節（あるいは腰関節）は関節１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）から、足関節は関節１８Ｒ（Ｌ），２０Ｒ（Ｌ）から構成される。また股関節と膝関節とは大腿リンク２４Ｒ，２４Ｌ、膝関節と足関節とは下腿リンク２６Ｒ，２６Ｌで連結される。
【００３５】
更に、腰部の上位には上体（あるいは基体。リンクで示す）２８が設けられると共に、その上端には左右それぞれの７個の関節からなる腕リンク３を備える（同様に、理解の便宜のために各関節をそれを駆動する電動モータで示す）。
【００３６】
７個の関節は上から順に、肩部のロール軸の関節３０Ｒ，３０Ｌ、同ピッチ軸の関節３２Ｒ，３２Ｌ、腕の回旋用の関節３４Ｒ，３４Ｌ、肘部のロール軸の関節３６Ｒ，３６Ｌ、手首回旋用の関節３８Ｒ，３８Ｌ、同ロール軸の関節４０Ｒ，４０Ｌ、および同ピッチ軸の関節４２Ｒ，４２Ｌから構成される。手首の先にはハンド（エンドエフェクタ）４４Ｒ，４４Ｌが取着される。
【００３７】
上記において肩関節は関節３０Ｒ（Ｌ），３２Ｒ（Ｌ），３４Ｒ（Ｌ）から、手首関節は関節３８Ｒ（Ｌ），４０Ｒ（Ｌ），４２Ｒ（Ｌ）から構成される。また肩関節と肘関節とは上腕リンク４６Ｒ，４６Ｌ、肘関節と手首関節とは下腕リンク４８Ｒ，４８Ｌで連結される。
【００３８】
尚、上体（基体）２８の内部には、図２に関して後述するマイクロコンピュータからなる制御ユニット５０などが格納される。
【００３９】
上記の構成により、脚リンク２は左右の足についてそれぞれ６つの自由度を与えられ、歩行中にこれらの６＊２＝１２個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に３次元空間を歩行させることができる（この明細書で「＊」は乗算を示す）。尚、前記の如く、ロボットの進行方向（ピッチ軸）をＸ軸、左右方向（ロール軸）をＹ軸、鉛直方向（重力軸）をＺ軸とする。
【００４０】
また、腕リンク３は左右の腕についてそれぞれ７つの自由度を与えられ、これらの７＊２＝１４個の関節を適宜な角度で駆動することで、後述する台車を押すなどの所望の作業を行うことができる。
【００４１】
図１に示す如く、足関節の下方の足平２２Ｒ（Ｌ）には公知の６軸力センサ５６が取着され、ロボットに作用する外力の内、接地面からロボットに作用する床反力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚとを検出する。
【００４２】
更に、手首関節とハンド４４Ｒ（Ｌ）の間には同種の６軸力センサ５８が取着され、ロボットに作用するそれ以外の外力、特に作業対象物から受ける前記した対象物反力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚとを検出する。
【００４３】
また、上体２８には傾斜センサ６０が設置され、Ｚ軸（鉛直軸（重力軸））に対する傾きとその角速度を検出する。また各関節の電動モータはその出力を減速・増力する減速機（図示せず）を介して前記したリンク２４，２６Ｒ（Ｌ）などを相対変位させると共に、その回転量を検出するロータリエンコーダが設けられて変位検出器付の脚アクチュエータあるいは腕アクチュエータとして構成される。これら６軸力センサ５６などの出力は制御ユニット５０に送られる（図示の便宜のためロボット１の右側についてのみ図示する）。
【００４４】
図２は制御ユニット５０の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ６０などの出力はＡ／Ｄ変換器７０でデジタル値に変換され、その出力はバス７２を介してＲＡＭ７４に送られる。また各アクチュエータにおいて電動モータに隣接して配置されるエンコーダの出力は、カウンタ７６を介してＲＡＭ７４内に入力される。
【００４５】
制御ユニット内にはＣＰＵからなる演算装置８０が設けられており、演算装置８０は後述の如く、ＲＯＭ８４に格納されている歩容に基づいてロボットが安定な姿勢を継続することができるように、関節角変位指令（アクチュエータ変位指令）を算出し、ＲＡＭ７４に送出する。
【００４６】
また演算装置８０はＲＡＭ７４からその指令と検出された実測値とを読み出し、各関節の駆動に必要な制御値（操作量）を算出してＤ／Ａ変換器８６と各関節に設けられたアクチュエータ駆動装置（アンプ）８８を介して各関節を駆動する脚アクチュエータと腕アクチュエータの電動モータに出力する。
【００４７】
図３は、この発明に係る脚式移動ロボットの姿勢制御装置（主として前記した演算装置８０に相当）の構成および動作を機能的に示すブロック図である。
【００４８】
この装置は脚および腕の動作を統合的に制御する装置であり、各アクチュエータ駆動装置８８に対する変位指令を出力する。図示の如く、この装置は、目標作業パターン生成器、対象物反力平衡制御装置、脚メイン制御装置、および腕メイン制御装置から構成される。
【００４９】
以下に、理解の便宜のため、図４に示すロボット作業状況を例に挙げて、この装置の各構成要素の処理内容を説明する。図４では、ロボット１が台車１００を押しているとき、台車から受ける実対象物反力の絶対値が目標作業パターンにおいて想定していた目標対象物反力よりも突然小さくなってしまったため、このずれによってロボット１はバランスを崩し、前に傾きかけている状況とする。この実施の形態に係る装置は、このような状況においても常に動バランスを維持するように制御するものである。
【００５０】
目標作業パターン生成器は、ある想定条件下において動力学的平衡条件を満足する目標作業パターンを生成する。目標作業パターンは、複数の変数の時間変化パターンによって表現される。この変数は、運動を表現する変数と環境から受ける反力を表現する変数から構成される。
【００５１】
ここで、運動を表現する変数は、これによって各瞬間における姿勢が一義的に決定できる変数の組である。具体的には、目標足平位置・姿勢、目標上***置・姿勢、目標ハンド位置・姿勢から構成される。
【００５２】
また、環境から受ける反力を表現する変数は、目標全床反力中心点（位置）（目標ＺＭＰ（位置））、目標全床反力および目標対象物反力から構成される。
【００５３】
これら各変数は、支持脚座標系で表される。支持脚座標系は、支持脚足首（関節１８，２０Ｒ（Ｌ）の交点）から足平２２Ｒ（Ｌ）への垂直投影点を原点とする座標系であり、図５および図６に示すように、支持脚が接触している床に固定された座標系であり、支持脚足平の前向きをＸ軸の向き、左向きをＹ軸の向き、鉛直方向上向きをＺ軸向きとする座標系である。
【００５４】
以下に、これら各変数について詳細を説明する。
【００５５】
前記の如く、ロボットが環境から受ける外力の内で、各足平床反力を除いた外力を対象物反力と呼ぶとき、目標対象物反力はその目標値である。図４の例ではハンド４４Ｒ（Ｌ）が対象物１００から受ける反力のことである。
【００５６】
目標作業パターン生成器が出力する目標対象物反力は、後述する目標全床反力中心点まわりに作用する力とモーメントによって表現される。ちなみに、姿勢安定化にとって重要なのは、このうちのモーメント成分である。
【００５７】
目標全床反力と目標全床反力中心点（位置）について説明すると、作業中において各足平が床から受けるべき目標床反力の合力を、広義の目標全床反力と呼ぶ。広義の目標全床反力は、目標全床反力中心点とその点における力とモーメントで表現される。目標全床反力中心点は、目標全床反力をその点を作用点とする力とモーメントで表現したとき、Ｘ軸まわりモーメント成分とＹ軸まわりモーメント成分が０になる床面上の点である。
【００５８】
狭義の目標全床反力は、広義の目標全床反力を、目標全床反力中心点を作用点として、力とモーメントで表現した場合の力とモーメントを意味する。目標作業パターン生成器が出力する目標全床反力は、狭義の目標全床反力である。
【００５９】
以降は特に説明がない限り、目標全床反力は、狭義の目標全床反力を指す。尚、平坦な床面を歩行する場合には、目標全床反力の作用点は、通常、その床面上に設定される。
【００６０】
歩行制御の分野において従来から公知であるＺＭＰの概念も、概念を次のように拡張する。即ち、ロボットの運動によって生じる慣性力と重力と対象物反力の合力が、その点を作用点とする力とモーメントで表現されたとき、Ｘ軸まわりモーメント成分とＹ軸まわりモーメント成分が０になる床面上の点を、ＺＭＰと呼ぶ。ロボットが目標の運動を行う時のＺＭＰを目標ＺＭＰ（位置）と呼ぶ。
【００６１】
目標作業パターンが動力学的平衡条件を満足すると言うことは、目標作業パターンによって生じる上記の慣性力と重力と対象物反力の合力と目標全床反力が、打ち消し合って０になることである。従って、動力学的平衡条件を満足するためには、目標全床反力中心点と目標ＺＭＰが一致しなければならない。
【００６２】
目標作業パターン生成器では、動力学的平衡条件を満足する目標作業パターンを生成する。従って、目標作業パターン生成器が生成する目標全床反力中心点（位置）は目標ＺＭＰ（位置）に一致する。
【００６３】
目標足平位置・姿勢、目標上***置・姿勢、目標ハンド位置・姿勢は、前記した支持脚座標系で表現されたそれぞれの部位の位置と姿勢を表す。具体的にはこの明細書で、上体２８の位置およびその速度は、上体２８の重心位置などの代表点およびその（変位）速度を意味する。更に、上体あるいは足平の姿勢は、Ｘ，Ｙ，Ｚ空間における『向き』を意味する。
【００６４】
対象物反力平衡制御装置およびその制御はこの実施の形態の制御の中心をなすもので、対象物反力平衡制御装置は姿勢バランスをとるために動力学的平衡条件を考慮しながら制御を行う。そこで、対象物反力平衡制御装置の概要を説明する前に、動力学的平衡条件について以下に説明する。
【００６５】
実際のロボットの姿勢傾きの挙動を決定する最も大きな要因は、目標全床反力中心点（即ち、目標ＺＭＰ）まわりでの実際の力のモーメントのバランスである。
【００６６】
目標全床反力中心点まわりに作用する力のモーメントを以下に列挙する。
１）慣性力モーメント
２）重力モーメント
３）全床反力モーメント
４）対象物反力モーメント
【００６７】
以上のモーメントは先にも説明したが、改めて以下に定義する。
【００６８】
慣性力モーメントは、目標全床反力中心点まわりのロボットの角運動量の変化によって生じるモーメントである。この値はオイラー方程式によって求められ、具体的には目標全床反力中心点まわりのロボットの角運動量の１階微分値の符号を反転させたものである。
【００６９】
目標作業パターンの慣性力モーメントを、目標慣性力モーメントと呼ぶ。実際のロボットが作業しているときの慣性力モーメントを実慣性力モーメントと呼ぶ。
【００７０】
重力モーメントは、ロボットの重心に作用する重力が目標全床反力中心点まわりに作用するモーメントである。
【００７１】
各足平に作用する床反力の合力を、全床反力と呼ぶ。全床反力モーメントは、全床反力が目標全床反力中心点まわりに作用するモーメントである。
【００７２】
作業対象物から受ける反力を、対象物反力と呼ぶ。対象物反力モーメントは、作業対象物反力が目標全床反力中心点まわりに作用するモーメントである。
【００７３】
さて、理想的な脚メイン制御装置によって、ロボット１が目標作業パターンの運動パターンに忠実に追従していたと仮定する。このときには実慣性力モーメントは目標慣性力モーメントに一致し、実重力モーメントは目標重力モーメントに一致する。
【００７４】
一方、動力学の法則（オイラー方程式）により、必ず実慣性力モーメントと実重力モーメントと実全床反力モーメントと実対象物反力モーメントの和は、０である。
【００７５】
故に、ロボット１が忠実に目標作業パターンの運動パターン通りに動くためには、目標慣性力モーメントと目標重力モーメントと実全床反力モーメントと実対象物反力モーメントの和が０でなければならない。これを条件１とする。
【００７６】
ところが、実際には、実対象物反力モーメントが目標対象物反力モーメントと一致せず差が生じる。例えば、図４に関して述べたように、台車を押す作業を行っているときに台車（すなわち目標対象物）の実際のころがり摩擦力の絶対値が想定していた値よりも突然小さくなってしまった状況である。
【００７７】
この図の状況では、実対象物反力が目標全床反力中心点のＹ軸まわりに作用するモーメントは、目標対象物反力が目標全床反力中心点のＹ軸まわりに作用するモーメントよりも正の向きに大きくなって条件１を満たさなくなり、ロボット１は前傾する。尚、モーメントの向きは、座標軸の正方向に向いてロボット１を時計まわりに回転させるモーメントを正とする。
【００７８】
このような状況においても条件１を満足させるためには、次の２通りの手法が考えられる。
【００７９】
手法１）上記偏差を打ち消すように、実全床反力モーメントを変える。具体的には、目標全床反力中心点まわりに負の床反力モーメントを発生するように脚メイン制御装置に指令し、脚メイン制御装置において、この指令を受けて、足平２２Ｒ（Ｌ）のつまさきを下げ、実全床反力モーメントを負の向きに増加させる。即ち、足で踏ん張るような姿勢をとらせる。
【００８０】
手法２）上記偏差を打ち消すように、目標作業パターンの運動パターンを修正することにより、目標慣性力モーメントと目標重力モーメントを修正する。具体的には、目標上***置および／または姿勢を修正することによって、目標慣性力モーメントと目標重力モーメントを修正する。即ち、上体を前に移動させる。
【００８１】
この実施の形態に係る装置では両方の手法を同時に行い、短期的には手法１を主に使うことによって速い変化に対応し、長期的には手法２を主に使うことによって実全床反力モーメントを元の目標全床反力モーメントに収束させながら、常に動バランスを維持するようにした。
【００８２】
実全床反力モーメントは、目標全床反力モーメントを変えるだけで脚メイン制御装置によってすばやく変化させることができるので、手法１は短期的な対応に向いている。但し、実全床反力モーメントを大きく変化させると、足平２２Ｒ（Ｌ）の接地圧分布が偏って接地感が減少し、最悪の場合には足平２２Ｒ（Ｌ）の一部が浮いてしまう。従って、長期的には、なるべく元の目標全床反力モーメントに戻すべきである。
【００８３】
実全床反力モーメントを元の目標全床反力モーメントに戻すためには、重心位置をずらし、目標重力モーメントによって上記偏差を打ち消すように、手法２によって目標作業パターンの運動パターンを修正すれば良い。但し、重心位置を急激にずらすと、過大な目標慣性力モーメントが逆向きに発生するので、ゆっくりと重心位置をずらす必要がある。従って、手法２は長期的な対応に向いている。
【００８４】
上記を前提として対象物反力平衡制御装置について説明する。対象物反力平衡制御装置は、上記の制御機能を持った装置である。
【００８５】
対象物反力平衡制御装置の入力は、目標上***置・姿勢、目標全床反力中心点（位置）、目標対象物反力、６軸力センサ５８の検出値、最終修正目標ハンド位置・姿勢、最終修正目標上***置・姿勢、最終修正目標足平位置・姿勢である（尚、近似演算を用いる場合は、最終修正目標ハンド位置・姿勢、最終修正目標上***置・姿勢、最終修正目標足平位置・姿勢は不要である）。
【００８６】
対象物反力平衡制御装置では、上記の制御機能を実現するために、目標対象物反力を実対象物反力の検出値に置き換え、それに動力学的に平衡するように目標上***置・姿勢と目標全床反力を修正する。これにより修正された作業パターンが想定している対象物反力（即ち、修正された目標対象物反力）と実対象物反力が一致し、ロボットの動力学的平衡条件が満足される。
【００８７】
対象物反力平衡制御装置の出力は、修正目標上***置・姿勢と対象物反力平衡制御用補償全床反力である。
【００８８】
修正目標上***置・姿勢は、対象物反力平衡制御装置によって修正された目標上***置・姿勢である。対象物反力平衡制御用補償全床反力は、目標全床反力中心点（位置）に、修正によって加えられる全床反力である。尚、対象物反力平衡制御用補償全床反力の成分の内で、姿勢安定化のための特に重要な成分は、Ｘ軸まわりモーメント成分とＹ軸まわりモーメント成分である。
【００８９】
対象物反力平衡制御装置の出力の挙動だけを述べると、実対象物反力と目標対象物反力の偏差、より正確には両者のモーメントの偏差が急変、即ち、図７に示すようにステップ状に変化する場合には、動力学平衡条件を満足するために、最初は対象物反力平衡制御用の補償全床反力のモーメント成分が、この差に応じてすばやく応答する。
【００９０】
その後しばらくすると、修正目標上***置・姿勢が、この偏差に静的に釣り合う位置・姿勢に整定し、対象物反力平衡制御用補償全床反力のモーメント成分は０に収束する。尚、対象物反力平衡制御装置の構成とアルゴリズム説明は後述する。
【００９１】
図３において、脚メイン制御装置に入力される目標値は、修正目標上***置・姿勢、目標足平位置・姿勢、目標全床反力中心点（位置）とその点に作用する目標全床反力と対象物反力平衡制御用補償全床反力である。
【００９２】
脚メイン制御装置の機能は、簡単に言うならば、脚のアクチュエータ（関節１０Ｒ（Ｌ）などの電動モータおよびエンコーダ）を操作し、目標姿勢に追従する姿勢安定化制御と目標床反力に追従する床反力制御を同時に行う装置である。尚、目標姿勢と目標床反力を同時に完全に満足させることは不可能であるので、適当な調整が行われ、長期的には両方を満足するように制御される。
【００９３】
より詳しくは、傾斜センサ６０によって検出された実上***置・姿勢を修正目標上***置・姿勢に復元させるために、目標全床反力中心点に発生させるべき復元全床反力を算出し、目標全床反力中心点に作用する実全床反力のモーメント成分が、この復元全床反力と目標全床反力と対象物反力平衡制御用補償全床反力の合力のモーメント成分に一致するように、足平２２Ｒ（Ｌ）を回転あるいは上下動させるべく目標足平位置・姿勢を修正する。修正された目標足平位置・姿勢を最終修正目標足平位置・姿勢と呼ぶ。
【００９４】
従って、傾斜センサ６０によって検出される実上***置・姿勢とその変化率が、修正目標上***置・姿勢とその変化率に一致しているならば、目標全床反力中心点位置に作用する実全床反力のモーメント成分が目標全床反力と対象物反力平衡制御用補償全床反力の合力のモーメント成分に一致するように目標足平位置・姿勢を修正する。
【００９５】
脚メイン制御装置は、さらに、修正目標上***置・姿勢と修正目標足平位置・姿勢から決定される目標脚関節変位に実関節変位が追従するように脚アクチュエータを制御する。
【００９６】
脚メイン制御系は脚メイン制御装置、および前記した傾斜センサ６０、足平２２Ｒ（Ｌ）に設けた６軸力センサ５６、アクチュエータ駆動装置８８およびアクチュエータ（関節１０Ｒ（Ｌ）ないし２０Ｒ（Ｌ）用電動モータおよびエンコーダ）から構成される。
【００９７】
脚メイン制御装置で修正された目標足平位置・姿勢は、最終修正目標足平位置・姿勢として対象物反力平衡制御装置に送られる。但し、対象物反力平衡制御装置において、目標足平位置・姿勢が修正されたことによるロボットの重心位置の変化が無視できるならば、最終修正目標足平位置・姿勢を対象物反力平衡制御装置に送る必要はない。
【００９８】
図３において、腕メイン制御装置に入力される目標値は、修正目標上***置・姿勢、目標ハンド位置・姿勢および目標対象物反力である。
【００９９】
腕メイン制御装置の機能は簡単に言うならば、腕のアクチュエータ（関節３０Ｒ（Ｌ）などの電動モータ、エンコーダ他）を操作して、目標姿勢に追従する姿勢制御と目標対象物反力に追従する対象物反力制御を同時に行うことである。目標姿勢と目標対象物反力を同時に完全に満足させることは不可能であるので、適宜な手法、例えば、従来からマニピュレータのコンプライアンス制御、いわゆる仮想コンプライアンス制御として知られるものを用いる（機械工学便覧、エンジニアリング編、Ｃ４−１００頁）。
【０１００】
具体的な制御系構成とアルゴリズムを以下に説明すると、腕メイン制御系は腕メイン制御装置、および前記したハンド４４（Ｌ）に設けた６軸力センサ５８、アクチュエータ駆動装置８８および腕アクチュエータ（関節３０Ｒ（Ｌ）ないし４２Ｒ（Ｌ）用電動モータおよびエンコーダ）から構成される。
【０１０１】
腕メイン制御装置は、６軸力センサ５８によって検出される実対象物反力と目標対象物反力の差に応じて目標ハンド位置・姿勢を修正する。修正された目標ハンド位置・姿勢を、最終修正目標ハンド位置・姿勢と呼ぶ。腕メイン制御装置は、修正目標上***置・姿勢と最終修正目標ハンド位置・姿勢から決定される目標腕関節変位に実関節変位が追従するように腕アクチュエータを制御する。
【０１０２】
ここで、対象物反力平衡制御装置の詳細を説明する。
【０１０３】
図８は対象物反力平衡制御装置の制御構成図の前半部分を、図９は対象物反力平衡制御装置の制御構成図の後半部分を示す機能ブロック図である。
【０１０４】
図８を参照して前半部分の処理から説明する。
【０１０５】
先ず、実際のハンド４４Ｒ（Ｌ）は、腕メイン制御装置によって、ほぼ、最終修正目標ハンド位置・姿勢にあると考えられるので、６軸力センサ５８によって検出された実対象物反力を、修正目標ハンド位置・姿勢によって支持脚座標系の原点まわりの力とモーメントに変換する（実関節変位からキネマティクス演算によって、実ハンド位置・姿勢を求めて、これを用いて実対象物反力を変換しても良い）。
【０１０６】
次に、変換された実対象物反力を、目標全床反力中心点まわりの力とモーメントに変換することにより、目標全床反力中心点まわりの実対象物反力モーメントを得る。最後に、これから目標全床反力中心点まわりの目標対象物反力モーメントを引くことにより、目標全床反力中心点まわりの対象物反力モーメント偏差を得る。
【０１０７】
次に図９を参照して対象物反力平衡制御装置の後半部分の処理を説明する。
【０１０８】
先ず、そこで用いる摂動動力学モデルについて説明する。
【０１０９】
摂動動力学モデルは、目標作業パターンの運動（摂動）にある拘束条件を与えておいた場合の、目標全床反力モーメント摂動量と上***置・姿勢摂動量との関係を表すモデルである。以下に、例として、図１０に示すようにロボットの上体姿勢を目標上体姿勢に一致させたまま、上体の水平位置を摂動するモデルを説明する。
【０１１０】
ここで、以下のように記号をとりきめる。
ｍ：ロボット全質量
ｇ：重力加速度
ｈ：目標全床反力中心点からの重心高さ
ΔｘＧ：目標重心位置摂動量のＸ成分
ΔｙＧ：目標重心位置摂動量のＹ成分
Δｘｂ：目標上***置摂動量のＸ成分
Δｙｂ：目標上***置摂動量のＹ成分
ΔＭｘ：目標全床反力中心点まわりの目標全床反力モーメント摂動量のＸ成分
ΔＭｙ：目標全床反力中心点まわりの目標全床反力モーメント摂動量のＹ成分
ΔＭＧｘ：目標全床反力中心点まわりの目標重力モーメント摂動量のＸ成分
ΔＭＧｙ：目標全床反力中心点まわりの目標重力モーメント摂動量のＹ成分
ΔＬｘ：目標全床反力中心点まわりの目標角運動量の摂動量のＸ成分
ΔＬｙ：目標全床反力中心点まわりの目標角運動量の摂動量のＹ成分
d(a) / dt：変数aの時間微分
d( d(a) / dt) /dt：変数aの時間２階微分
【０１１１】
重力モーメントの定義から、次式が導かれる。

【０１１２】
ロボットの運動摂動に関する重心まわりの等価慣性モーメントが十分小さく無視できるならば、次式が導かれる。

【０１１３】
オイラー方程式により、次式が導かれる。

【０１１４】
式１、式２および式３より、摂動動力学モデルの運動方程式として、次式を得ることができる。

【０１１５】
ところで、目標重心位置摂動量と目標上***置摂動量は、ほぼ比例関係にあると考えられる。従って、比例定数をｋとすると、次式により目標上***置摂動量が得られる。

【０１１６】
以上から、摂動動力学モデルは、式４と式５を用い、目標重心位置摂動量と目標上***置摂動量を算出する。詳しくはこの装置の如く、デジタル演算の場合には、式４は離散化して使用する。ちなみに、式４は、図１１に示す、高さｈ、質量ｍの倒立振子の運動方程式に一致する。
【０１１７】
図９に示す対象物反力平衡制御装置後半部において、前記した目標全床反力中心点まわりの対象物反力モーメント偏差は、最終到達目標重心摂動量算出部に入力される。
【０１１８】
この目標全床反力中心点まわりの対象物反力モーメント偏差を長期的に打ち消してバランスをとるための重心摂動量を最終到達目標重心位置摂動量と呼ぶ。最終到達目標重心位置摂動量算出部は、上記偏差から最終到達目標重心位置摂動量を算出する。
【０１１９】
ここで、
ΔＭｏｘ：対象物反力モーメント偏差のＸ成分
ΔＭｏｙ：対象物反力モーメント偏差のＹ成分
ΔＭＧｏｘ：最終到達目標重心位置摂動量によって発生する重力モーメントのＸ成分
ΔＭＧｏｙ：最終到達目標重心位置摂動量によって発生する重力モーメントのＹ成分
ΔｘＧe：最終到達目標重心位置摂動量のＸ成分
ΔｙＧe：最終到達目標重心位置摂動量のＹ成分
とする。
【０１２０】
対象物反力モーメント偏差を最終到達目標重心位置摂動量によって発生する重力モーメントによって打ち消すためには、次式を満足する必要がある。

【０１２１】
最終到達目標重心位置摂動量によって発生する重力モーメントは、次式のようになる。

【０１２２】
式６、式７より次式を得る。

【０１２３】
故に、最終到達目標重心位置摂動量は、式８によって算出すれば良い。
【０１２４】
対象物反力平衡制御装置のモデル制御則演算器について説明すると、最終到達目標重心位置摂動量と摂動動力学モデルが出力する目標重心位置摂動量との差を、重心変位偏差と呼ぶ。モデル制御則演算器は、この重心変位偏差を０に収束させるための制御を行う。出力は、対象物反力平衡制御用補償全床反力モーメントである。
【０１２５】
具体的には、次式のような、ＰＤ制御則によって、対象物反力平衡制御用補償全床反力モーメントを決定すれば良い。

ここで、Ｋｐは比例ゲイン、Ｋｄは微分ゲインである。
【０１２６】
モデル制御則演算器の出力直後の加算点について説明すると、モデル制御則演算器の出力直後の加算点によって、摂動動力学モデルには、対象物反力平衡制御用補償全床反力モーメントと、目標全床反力中心点まわりの対象物反力モーメント偏差の和が、モデルのための目標全床反力モーメント摂動量（モデル入力量）として入力され、その入力に対応する目標上***置・姿勢摂動量が算出される。これが目標上***置・姿勢に加算され、修正目標上***置・姿勢が作られる。
【０１２７】
ところで、摂動動力学モデルは動力学的平衡条件を満たすので、モデル出力である目標上***置・姿勢摂動量によって発生する目標慣性力モーメント摂動量および目標重力モーメント摂動量と、モデル入力との和は０である。
【０１２８】
故に、次式が成立する。

【０１２９】
一方、脚メイン制御装置には、目標全床反力中心点まわりに対象物反力平衡制御用補償全床反力モーメントを付加的に発生するように指令が送られる。即ち、実全床反力モーメントに、実全床反力モーメント摂動量として、対象物反力平衡制御用補償全床反力モーメントが加えられるように制御される。その結果発生する、実全床反力モーメントを修正実全床反力モーメントと呼ぶ。
【０１３０】
故に、次式が成立する。

【０１３１】
式１１と式１２から、次式が得られる。

【０１３２】
ところで、目標作業パターンは動力学的平衡条件を満足しているから、次式を満足する。

【０１３３】
各修正モーメントが元のモーメントにモーメント摂動量を加えたものであること、実対象物反力モーメントが目標対象物反力モーメントと対象物反力モーメント偏差の和であること、および式１３と式１４から、恒等式として次式が得られる。

【０１３４】
式１５は、実対象物反力モーメントがいかに目標対象物反力モーメントからずれようとも、対象物反力平衡制御によって目標慣性力モーメント、目標重力モーメントおよび実全床反力モーメントが修正され、常に条件１を満たしていることを意味する。
【０１３５】
同一のことを式１３を用いて言い換えると、対象物反力モーメント偏差が発生しても、対象物反力平衡制御によって、目標慣性力モーメント摂動量、目標重力モーメント摂動量および実全床反力モーメント摂動量が発生し、条件１を満足するように対象物反力モーメント偏差の影響を打ち消していると言える。
【０１３６】
図４の台車押し作業の状況に対する対象物反力平衡制御の挙動を、図７を再び参照して説明する。
【０１３７】
台車を押す作業を行っているとき、台車（即ち、目標対象物）の実際のころがり摩擦力の絶対値が想定していた値よりも突然ステップ状に小さくなってしまった状況では、対象物反力モーメント偏差も、図のようにステップ状に変化する。
【０１３８】
これに対し、最終到達目標重心位置摂動量算出部が、最終到達目標重心位置摂動量を算出する。モデル制御則演算器により最終到達目標重心位置摂動量と目標重心位置摂動量の差に応じて、対象物反力平衡制御用補償全床反力モーメントが算出される。
【０１３９】
図７に示すように、対象物反力平衡制御用補償全床反力モーメントは、目標重心位置摂動量が最終到達目標重心位置摂動量に漸近するにつれて０に漸近する。摂動動力学モデルには対象物反力平衡制御用補償全床反力モーメントと対象物反力モーメント偏差の和が入力され、目標重心位置摂動量と目標上***置・姿勢摂動量が、摂動動力学モデルから出力される。尚、この例では、姿勢を変えないことが拘束条件であるので、目標上***置・姿勢摂動量は０である。
【０１４０】
ところで、摂動動力学モデルが動力学的平衡条件を満足することから、目標上***置・姿勢摂動量によって発生する目標慣性力モーメント摂動量と目標重力モーメント摂動量の和に、摂動動力学モデルに入力されたモーメントを加えた総和は０である。
【０１４１】
即ち、目標慣性力モーメント摂動量、目標重力モーメント摂動量、対象物反力平衡制御用補償全床反力モーメントと対象物反力モーメント偏差の和は０になる。この関係は、図７に示すように、常に成立する。目標重心位置摂動量は、モデル制御則演算器によって最終到達目標重心位置摂動量に漸近させられる。目標重心位置摂動量は、目標重力モーメント摂動量に比例あるいはほぼ比例して変化する。
【０１４２】
以上が、対象物反力平衡制御装置の挙動である。尚、上記制御演算は、全て、制御周期毎に実行される。従って、実対象物反力モーメントがいつ変化しても、常に、動バランスは維持される。換言すれば、対象物反力モーメントが目標値からずれたとき、ロボット１は最初は全床反力モーメントを操作してつまさき（足平２２Ｒ（Ｌ）の先端）を踏ん張るように姿勢制御されると共に、経時的に上体を前方に移動させて重力モーメントに頼るように切り換えられる。
【０１４３】
図１２はこの発明の第２の実施の形態を示し、リミッタ２００を設け、最終到達目標重心位置摂動量に上限下限の制限値を設定してリミットをかけるようにしたものである。
【０１４４】
上記した実施の形態において、実際には、最終到達目標重心位置摂動量をあまり大きくすると、ロボットの姿勢がとれなくなる場合が生じる。従って、これを防ぐために、式７によって得られた最終到達目標重心位置摂動量に、上限下限の制限値（範囲）を設定してリミットをかけるようにした。尚、その制限値（リミット値）は固定値でも良く、あるいは可変値としても良い。
【０１４５】
更に、第２のリミッタ３００を設け、モデル制御則演算器で演算された対象物反力平衡制御用補償全床反力モーメントにも上限下限の制限値（範囲）を設定し、リミットをかけるようにした。
【０１４６】
即ち、対象物反力平衡制御用補償全床反力モーメントは実ロボットの足平２２Ｒ（Ｌ）にも発生させるが、実ロボットの足平が発生できる全床反力モーメントには限度があり、限度を超えると、足平の接地性が損なわれたり、足平の一部が床から浮いたりする。それを防ぐためには、モデル制御則演算器が式１０を用いて演算した対象物反力平衡制御用補償全床反力モーメントに上限下限の制限値を設定し、リミットをかけるようにした。その制限値が固定でも可変でも良いことはリミッタ２００の場合と同様である。
【０１４７】
図１３はこの発明の第３の実施の形態を示し、リミッタ４００を設けて最終到達目標重心位置摂動量算出部の入力に上限下限の制限値を設定してリミットをかけると共に、第２のリミッタ５００を設け、第１のリミッタ４００を超えた入力値で対象物反力平衡制御用補償全床反力モーメントを修正するようにした。
【０１４８】
図１２に示した第２の実施の形態においては、最終到達目標重心位置摂動量が過大になるのを防止するために、式７によって得られた最終到達目標重心位置摂動量に、上限下限の制限値（範囲）を設定してリミットをかけるようにしたが、制限値（リミット値）を超えた値がモデル制御則演算器の出力に加算点６００（図１２）において加算され、モデルに入力されることがあった。このため、対象物反力モーメント偏差に抗して重心位置を摂動していたものが、対象物反力モーメント偏差が過大になり、リミッタが作動すると、重心位置を逆方向に揺動してしまうと言う不都合を生じることがあった。
【０１４９】
第３の実施の形態は上記した不都合を解消するもので、第１のリミッタ４００の制限値（リミット値）を超えた入力値を第２のリミッタ５００を通して加減算点７００に送り、そこでモデル制御則演算器の出力から減算、換言すれば極性を反転させて転倒させようとする力と逆向きの力を与えるように、対象物反力平衡制御用補償全床反力モーメントを修正するようにした。これによって、ロボットは制限値（リミット値）を超えた入力値（モーメント偏差）を足平２２Ｒ（Ｌ）で支持するように姿勢制御される。
【０１５０】
第２の実施の形態と異なり、リミット値を超えた値がモデル制御則演算器の出力に加算点７１０（図１３）において加算されてモデルに入力されることがない。このため、前記したような対象物反力モーメント偏差が過大になってリミッタが作動し、重心位置を逆方向に揺動してしまうと言う不都合を解消することができる。尚、残余の構成は従前の実施の形態と異ならない。第１のリミッタ４００の制限値（リミット値）が固定でも可変でも良いことも、従前の実施の形態と同様である。さらに、モデル制御則演算器の後に、リミッタ４００と同様のリミッタを追加しても良い。
【０１５１】
図１４はこの発明の第４の実施の形態を示し、対象物反力平衡制御装置において摂動動力学モデルの精度を高めるために、慣性力モーメントＩを与えた倒立振子モデルを使用するようにした。
【０１５２】
更に、摂動動力学モデルについて敷衍すると、重心高さがあまり変わらないならば、ｈは固定で良いが、作業によって重心高さが変わる場合には、最終修正目標上***置・姿勢、最終修正目標足平位置・姿勢および最終修正目標ハンド位置・姿勢から求められるロボット姿勢から重心高さを求め、これに応じてｈを変更しても良い。
【０１５３】
また、摂動動力学モデルの精度をより高めるために、脚腕のリンクを持つロボットの多リンク幾何学モデルを備え、最終修正目標上***置・姿勢、最終修正目標足平位置・姿勢および最終修正目標ハンド位置・姿勢から求められる重心位置と、最終修正目標上***置・姿勢から上***置の摂動量を差し引いた上***置・姿勢、最終修正目標足平位置・姿勢および最終修正目標ハンド位置・姿勢から求められる重心位置との差を求めることにより、高精度な重心位置の摂動量と上***置の摂動量との関係を求め、それを用いて重心位置の摂動量から上***置の摂動量を求めても良い。
【０１５４】
また、脚腕のリンクを持つロボットの多リンク動力学モデルであって、運動パターンにある拘束条件を与えておいて、目標床反力の摂動を入力として目標上***置・姿勢摂動量と重心位置・姿勢摂動量を出力させるモデルを用いても良い。
【０１５５】
ところで、腕を目標作業パターンから摂動させたときの腕の慣性力摂動量およびまたは重力摂動量の影響を考慮した摂動動力学モデルを用いると、制御装置の負荷が大きくなる。
【０１５６】
何故なら、腕の慣性力摂動量およびまたは重力摂動量は、目標上***置・姿勢摂動量に影響され、逆に目標上***置・姿勢摂動量は、腕の慣性力摂動量およびまたは重力摂動量に影響されるので、この相互作用を同時に考慮してモデル挙動を算出することは、非常に複雑な演算が必要となるからである。
【０１５７】
その問題を解決する手段として、以下の手法を用いても良い。
【０１５８】
即ち、摂動動力学モデルでは、腕を目標作業パターンから摂動させたときの腕の慣性力摂動量およびまたは重力摂動量の影響を無視し、腕は目標動作パターン通りにしか動かないものと仮定する。この仮定により、モデルは摂動動力学モデルの詳細説明に例として挙げた倒立振子モデルと同一形式に近似される。従って、摂動動力学モデルの演算は極めて簡単になる。
【０１５９】
腕メイン制御装置において、目標ハンド位置・姿勢、目標上***置、最終修正目標ハンド位置・姿勢および最終修正目標上***置から、腕を目標姿勢から最終目標姿勢に摂動させたために生じた慣性力摂動量およびまたは重力摂動量を算出する。これは、従来からの手法である多リンクマニピュレータの動力学演算を行うことにより得られる。これは、最終修正目標ハンド位置・姿勢座標系で表す。
【０１６０】
算出された慣性力摂動量およびまたは重力摂動量を、センサによって検出された実対象物反力に加え、対象物反力平衡制御装置に実対象物反力として出力する。以上の手段により、腕の慣性力摂動量およびまたは重力摂動量の影響を、摂動動力学モデルにおいて無視した代わりに、作業対象物反力として考慮したこととなる。腕の慣性力摂動量およびまたは重力摂動量の演算と摂動動力学モデルの演算が独立して行われるので、複雑な干渉演算が不要となり、演算量が小さくて済む。
【０１６１】
上記の如く、第１ないし第４の実施の形態にあっては、少なくとも基体（上体２８）と、前記基体に連結される複数本のリンク（脚リンク２および腕リンク３）からなる脚式移動ロボット（２足歩行ロボット１）の姿勢制御装置において、前記ロボットの少なくとも前記基体の目標軌跡を含む運動パターンと、前記ロボットに作用する床反力の目標軌跡と、前記ロボットに作用する床反力以外の外力の目標軌跡とを少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段（目標作業パターン生成器）、前記床反力以外の外力を検出する外力検出手段（６軸力センサ５８）、前記検出された外力と、前記目標軌跡で設定された床反力以外の外力の偏差（目標全床反力中心点まわりの対象物反力モーメント偏差）を演算する外力偏差演算手段（対象物反力平衡制御装置。より具体的には、図８の実対象物反力の座標変換およびその入出力）、前記床反力の摂動と前記ロボットの重心位置および／または基体の位置の摂動の関係を表現するモデル（摂動動力学モデル）、少なくとも前記演算された外力の偏差に基づいて前記モデルに入力すべきモデル入力量（モデルのための目標全床反力モーメント摂動量）を演算するモデル入力量演算手段（モデル制御則演算器およびその後の加算点での入出力）、前記演算されたモデル入力量を前記モデルに入力し、得られる前記重心位置および／または基体の摂動量に応じて前記基体の目標軌跡を修正する、基体目標軌跡修正量（修正目標上***置・姿勢）を演算する基体目標軌跡修正量演算手段（対象物反力平衡制御装置。より具体的には、摂動動力学モデル入力量を入力し、モデルの挙動を演算し、モデル出力から目標上***置姿勢摂動量（修正量）を求める部分）、少なくとも前記演算されたモデル入力量に応じて前記床反力の目標軌跡を修正する、床反力目標軌跡修正量（対象物反力平衡制御用補償全床反力モーメント）を演算する床反力目標軌跡修正量演算手段（モデル制御則演算器、より具体的にはモデル制御則の一部）、および少なくとも前記演算された基体目標軌跡修正量および床反力目標軌跡修正量に基づいて前記ロボットの関節を変位させる関節変位手段（脚メイン制御装置、アクチュエータ駆動装置８８、脚アクチュエータなど）、を備える如く構成した。
【０１６２】
また、前記モデル入力量演算手段は、前記外力に静的に平衡する平衡重心位置の摂動量を算出する平衡重心位置摂動量算出手段（最終到達目標重心位置摂動量算出部）、を備え、前記算出された平衡重心位置に前記モデルが収束するように前記モデル入力量を演算する如く構成した。
【０１６３】
また、前記モデルが前記ロボットを倒立振子で近似するモデル（摂動動力学モデル）である如く構成した。
【０１６４】
また、前記平衡重心位置摂動量算出手段は、前記算出された平衡重心位置の摂動量を所定の範囲に制限するリミッタ２００，４００を備える如く構成した。
【０１６５】
また、前記床反力目標軌跡修正量演算手段は、前記演算された床反力目標軌跡修正量を所定の範囲に制限するリミッタ３００，５００を備えるように構成した。
【０１６６】
また、前記床反力の目標軌跡は、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む如く構成した。
【０１６７】
また、前記床反力目標軌跡修正量演算手段は、前記床反力目標軌跡修正量（対象物反力平衡制御補償用全床反力モーメント）が、前記モデル入力量（モデルのための目標全床反力モーメント摂動量）から前記外力の偏差（目標全床反力中心点まわりの対象物反力モーメント偏差）を減算した値と、前記床反力の目標中心点まわりに作用するモーメントに動力学的に釣り合うように、前記床反力目標軌跡修正量を演算する如く構成した。
【０１６８】
また、前記床反力以外の外力が、前記リンクを介して前記ロボットに作用する作業対象物（台車１００）からの反力である如く構成した。
【０１６９】
また、前記ロボットが、前記基体に連結される２本の脚リンク２と２本の腕リンク３からなる脚式移動ロボットである如く構成した。
【０１７０】
また、少なくとも基体（上体２８）と、前記基体に連結される複数本のリンク（脚リンク２、腕リンク３）からなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンと、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段（目標作業パターン生成器）、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段（６軸力センサ５８）、前記検出された対象物反力を前記目標床反力中心点まわりのモーメントとして変換する対象物反力モーメント変換手段（対象物反力平衡制御装置）、前記変換された対象物反力モーメントに動力学的に釣り合うように、前記目標中心点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段（対象物反力平衡制御装置）、および前記修正された目標中心点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段（脚メイン制御装置、アクチュエータ駆動装置８８、脚アクチュエータなど）、を備える如く構成した。
【０１７１】
また、少なくとも基体（上体２８）と、前記基体に連結される複数本のリンク（脚リンク２、腕リンク３）からなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンを設定する目標歩容設定手段（目標作業パターン生成器）、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段（６軸力センサ５８）、前記検出された対象物反力を所定の点、より具体的には目標床反力中心点まわりのモーメントとして変換する対象物反力モーメント変換手段（対象物反力平衡制御装置）、前記変換された対象物反力モーメントに動力学的につりあうように、前記所定の点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段（対象物反力平衡制御装置）、および前記修正された所定の点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段（脚メイン制御装置、アクチュエータ駆動装置８８、脚アクチュエータなど）、を備える如く構成した。
【０１７２】
尚、上記した第１ないし第４の実施の形態においては、上体リンクの曲げやひねりのためのアクチュエータを設けなかったが、それを追加するとき、上体アクチュエータ制御装置も必要となる。但し、上体リンクの曲げやひねりは腕または脚の付け根側に関節を追加したことと等価であるので、概念上、腕または脚のアクチュエータとみなすことができる。即ち、上体アクチュエータ制御装置は、腕または脚の制御装置の一部として含まれると考えることができる。
【０１７３】
上記した第１ないし第４の実施の形態では、先に特開平５−３０５５８６号公報で提案したコンプライアンス制御を用いているが、それ以外の手段を用いても良い。脚制御に、そのコンプライアンス制御以外の別の手段、たとえば、電動アクチュエータを電流指令型のアンプによって制御する手段を用いて関節トルクを制御し、その結果、間接的に床反力を制御する手段を用いれば、足平２２Ｒ（Ｌ）に設けた６軸力センサ５６は不要である。
【０１７４】
更に、上記した第１ないし第４の実施の形態において、腕の制御に、仮想コンプライアンス制御以外の別の手段、たとえば、電動アクチュエータを電流指令型のアンプによって制御する手段を用いて関節トルクを制御し、その結果、間接的に対象物反力を制御しても良い。その制御ではハンドの６軸力センサは不要であるが、対象物反力平衡制御装置のために、ハンドの６軸力センサは設けるのが良い。
【０１７５】
更に、上記した第１ないし第４の実施の形態において、ハンドの６軸力センサの代わりに、関節トルクから実対象物反力を推定する推定器を腕制御装置に備えても良い。この推定器は、従来技術である外乱オブザーバーを用いれば良い。
【０１７６】
更には、上記した第１ないし第４の実施の形態において、特開平５−３０５５８６号で提案したコンプライアンス制御に加えて、本出願人が特開平５−３３７８４９号公報で提案した制御を加えても良い。但し、その制御によって上体の位置や歩幅が修正されるので、腕制御においてハンドと作業対象物との相対位置関係が重要な場合には、その制御によって修正される上体の位置や歩幅の影響を考慮する必要がある。
【０１７７】
更には、上記した第１ないし第４の実施の形態において、床が平面でない場合でも、本出願人が特開平５−３１８８４０号公報で提案した仮想平面を想定する技術を用いて目標全床反力中心点や目標ＺＭＰを仮想平面上に求めても良い。
【０１７８】
更には、上記した第１ないし第４の実施の形態において、ロボット全体の姿勢が目標からずれて傾くと、ハンドの位置・姿勢が絶対空間においてずれる。この結果、対象物反力が目標対象物反力から大きくずれる場合がある。
【０１７９】
その問題点を解決するために、傾斜センサによって検出される実上***置・姿勢と目標上***置・姿勢のずれに応じて上記の修正された最終目標ハンド位置・姿勢をさらに補正することにより、ロボット全体の姿勢が傾いても、ハンドの位置・姿勢が絶対空間においてずれないようにするのが、より好ましい。
【０１８０】
更には、上記した第１ないし第４の実施の形態において、ブロック図は演算処理順序を変えるなど、種々の変形が可能である。
【０１８１】
更には、上記した第１ないし第４の実施の形態においてはＰＤ制御則を用いたが、それ以外の制御則（たとえば、PID制御、状態フィードバック制御）などを用いても良い。
【０１８２】
また、この発明を腕を備えた２足歩行の脚式移動ロボットについて説明したが、腕を備えない脚式移動ロボットにも有益であり、更に２足歩行ロボットに限らず、多脚ロボットにも応用することができる。
【０１８３】
【発明の効果】
脚式移動ロボットが予期できない外力、より具体的には作業対象物から反力を受けても、動バランスをとって安定な姿勢を継続することができる。更に、予め想定していなかった動作パターンでリンク、より具体的には腕を動かして作業をするときに腕に発生する重力、慣性力だけでなく、作業対象から予期せぬ反作用を受けても、動バランスをとって安定な姿勢を継続することができる。
【０１８４】
また、対象物反力が急変するときも、それに静的にバランスをとる位置にロボットの重心を移動させることにより、傾きや転倒を効果的に抑制することができる。また、ロボットの重心が移動する過渡期においても、重心位置や床反力を適正に変化させて動バランスを維持し続けることができる。
【図面の簡単な説明】
【図１】この発明に係る脚式移動ロボットの姿勢制御装置を全体的に示す説明図である。
【図２】図１に示す２足歩行ロボットの制御ユニットの詳細を示すブロック図である。
【図３】この発明に係る脚式移動ロボットの姿勢制御装置の構成および動作を機能的に示すブロック図である。
【図４】図１に示す脚式移動ロボットが腕を使用して行う作業を示す説明図である。
【図５】図３装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。
【図６】図５と同様に、図３装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。
【図７】図３に示す対象物反力平衡制御装置の動作を説明するタイミング・チャートである。
【図８】図３に示す対象物反力平衡制御装置の詳細な構成を示すブロック図の前半部である。
【図９】図３に示す対象物反力平衡制御装置の詳細な構成を示すブロック図の後半部である。
【図１０】図９に示す対象物反力平衡制御装置の摂動動力学モデルを示す説明図である。
【図１１】図１０に示すモデルを倒立振子で近似した状態を示す説明図である。
【図１２】図９に類似する、この発明の第２の実施の形態を示す対象物反力平衡制御装置の詳細な構成を示すブロック図の後半部である。
【図１３】図９に類似する、この発明の第３の実施の形態を示す対象物反力平衡制御装置の詳細な構成を示すブロック図の後半部である。
【図１４】図１１に類似する、この発明の第４の実施の形態を示す倒立振子型摂動動力学モデルを示す説明図である。
【符号の説明】
１ ２足歩行ロボット（脚式移動ロボット）
２ 脚リンク
３ 腕リンク
１０，１２，１４Ｒ，Ｌ 腰関節
１６Ｒ，Ｌ 膝関節
１８，２０Ｒ，Ｌ 足関節
２２Ｒ，Ｌ 足平
２８ 上体
３０，３２，３４Ｒ，Ｌ 肩関節
３６Ｒ，Ｌ 肘関節
３８，４０，４２Ｒ，Ｌ 手首関節
４４Ｒ，Ｌ ハンド
５０ 制御ユニット
５６，５８ ６軸力センサ
６０ 傾斜センサ

Claims (11)

1. 少なくとも基体と、前記基体に連結される複数本のリンクからなる脚式移動ロボットの姿勢制御装置において、
   ａ．前記ロボットの少なくとも前記基体の目標軌跡を含む運動パターンと、前記ロボットに作用する床反力の目標軌跡と、前記ロボットに作用する床反力以外の外力の目標軌跡とを少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段、
   ｂ．前記床反力以外の外力を検出する外力検出手段、
   ｃ．前記検出された外力と、前記目標軌跡で設定された床反力以外の外力との偏差を演算する外力偏差演算手段、
   ｄ．前記床反力の摂動と前記ロボットの重心位置および基体の位置の少なくともいずれかの摂動の関係を表現するモデル、
   ｅ．少なくとも前記演算された外力の偏差に基づいて前記モデルに入力すべきモデル入力量を演算するモデル入力量演算手段、
   ｆ．前記演算されたモデル入力量を前記モデルに入力し、得られる前記重心位置および基体の少なくともいずれかの摂動量に応じて前記基体の目標軌跡を修正する、基体目標軌跡修正量を演算する基体目標軌跡修正量演算手段、
   ｇ．少なくとも前記演算されたモデル入力量に応じて前記床反力の目標軌跡を修正する、床反力目標軌跡修正量を演算する床反力目標軌跡修正量演算手段、
   および
   ｈ．少なくとも前記演算された基体目標軌跡修正量および床反力目標軌跡修正量に基づいて前記ロボットの関節を変位させる関節変位手段、
   を備えたことを特徴とする脚式移動ロボットの姿勢制御装置。
2. 前記モデル入力量演算手段は、
   ｊ．前記外力に静的に平衡する平衡重心位置の摂動量を算出する平衡重心位置摂動量算出手段、
   を備え、前記算出された平衡重心位置に前記モデルが収束するように前記モデル入力量を演算することを特徴とする請求項１項記載の脚式移動ロボットの姿勢制御装置。
3. 前記モデルが前記ロボットを倒立振子で近似するモデルであることを特徴とする請求項１項または２項記載の脚式移動ロボットの姿勢制御装置。
4. 前記平衡重心位置摂動量算出手段は、
   ｋ．前記算出された平衡重心位置の摂動量を所定の範囲に制限するリミッタ、
   を備えることを特徴とする請求項２項または３項記載の脚式移動ロボットの姿勢制御装置。
5. 前記床反力目標軌跡修正量演算手段は、
   ｌ．前記演算された床反力目標軌跡修正量を所定の範囲に制限するリミッタ、
   を備えることを特徴とする請求項１項から４項のいずれかに記載の脚式移動ロボットの姿勢制御装置。
6. 前記床反力の目標軌跡は、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含むことを特徴とする請求項１項から５項記載の脚式移動ロボットの姿勢制御装置。
7. 前記床反力目標軌跡修正量演算手段は、前記床反力目標軌跡修正量が、前記モデル入力量から前記外力の偏差を減算した値と、前記床反力の目標中心点まわりに作用するモーメントに動力学的に釣り合うように、前記床反力目標軌跡修正量を演算することを特徴とする請求項６項記載の脚式移動ロボットの姿勢制御装置。
8. 前記床反力以外の外力が、前記リンクを介して前記ロボットに作用する作業対象物からの反力であることを特徴とする請求項１項から７項のいずれかに記載の脚式移動ロボットの姿勢制御装置。
9. 前記ロボットが、前記基体に連結される２本の脚リンクと２本の腕リンクからなる脚式移動ロボットであることを特徴とする請求項１項から７項のいずれかに記載の脚式移動ロボットの姿勢制御装置。
10. 少なくとも基体と、前記基体に連結される複数本のリンクからなる脚式移動ロボットの姿勢制御装置において、
    ａ．前記ロボットの少なくとも前記基体の目標位置を含む運動パターンと、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段、
    ｂ．前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段、
    ｃ．前記検出された対象物反力を前記目標中心点まわりのモーメントとして変換する対象物反力モーメント変換手段、
    ｄ．前記変換された対象物反力モーメントに動力学的に釣り合うように、前記目標中心点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段、
    および
    ｅ．前記修正された目標中心点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段、
    を備えたことを特徴とする脚式移動ロボットの姿勢制御装置。
11. 少なくとも基体と、前記基体に連結される複数本のリンクからなる脚式移動ロボットの姿勢制御装置において、
    ａ．前記ロボットの少なくとも前記基体の目標位置を含む運動パターンを設定する目標歩容設定手段、
    ｂ．前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段、
    ｃ．前記検出された対象物反力を所定の点まわりのモーメントとして変換する対象物反力モーメント変換手段、
    ｄ．前記変換された対象物反力モーメントに動力学的につりあうように、前記所定の点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段、
    および
    ｅ．前記修正された所定の点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段、
    を備えたことを特徴とする脚式移動ロボットの姿勢制御装置。

Images