JP3672426B2

JP3672426B2 - Posture control device for legged mobile robot

Info

Publication number: JP3672426B2
Application number: JP36441297A
Authority: JP
Inventors: 透竹中; 忠明長谷川; 隆志松本
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 1996-12-19
Filing date: 1997-12-19
Publication date: 2005-07-20
Anticipated expiration: 2017-12-19
Also published as: JPH10230485A

Description

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

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

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

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

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

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

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

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

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

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

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

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

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

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

【０１３４】
式１５は、実対象物反力モーメントがいかに目標対象物反力モーメントからずれようとも、対象物反力平衡制御によって目標慣性力モーメント、目標重力モーメントおよび実全床反力モーメントが修正され、常に条件１を満たしていることを意味する。
【０１３５】
同一のことを式１３を用いて言い換えると、対象物反力モーメント偏差が発生しても、対象物反力平衡制御によって、目標慣性力モーメント摂動量、目標重力モーメント摂動量および実全床反力モーメント摂動量が発生し、条件１を満足するように対象物反力モーメント偏差の影響を打ち消していると言える。
【０１３６】
図４の台車押し作業の状況に対する対象物反力平衡制御の挙動を、図７を再び参照して説明する。
【０１３７】
台車を押す作業を行っているとき、台車（即ち、目標対象物）の実際のころがり摩擦力の絶対値が想定していた値よりも突然ステップ状に小さくなってしまった状況では、対象物反力モーメント偏差も、図のようにステップ状に変化する。
【０１３８】
これに対し、最終到達目標重心位置摂動量算出部が、最終到達目標重心位置摂動量を算出する。モデル制御則演算器により最終到達目標重心位置摂動量と目標重心位置摂動量の差に応じて、対象物反力平衡制御用補償全床反力モーメントが算出される。
【０１３９】
図７に示すように、対象物反力平衡制御用補償全床反力モーメントは、目標重心位置摂動量が最終到達目標重心位置摂動量に漸近するにつれて０に漸近する。摂動動力学モデルには対象物反力平衡制御用補償全床反力モーメントと対象物反力モーメント偏差の和が入力され、目標重心位置摂動量と目標上体位置・姿勢摂動量が、摂動動力学モデルから出力される。尚、この例では、姿勢を変えないことが拘束条件であるので、目標上体位置・姿勢摂動量は０である。
【０１４０】
ところで、摂動動力学モデルが動力学的平衡条件を満足することから、目標上体位置・姿勢摂動量によって発生する目標慣性力モーメント摂動量と目標重力モーメント摂動量の和に、摂動動力学モデルに入力されたモーメントを加えた総和は０である。
【０１４１】
即ち、目標慣性力モーメント摂動量、目標重力モーメント摂動量、対象物反力平衡制御用補償全床反力モーメントと対象物反力モーメント偏差の和は０になる。この関係は、図７に示すように、常に成立する。目標重心位置摂動量は、モデル制御則演算器によって最終到達目標重心位置摂動量に漸近させられる。目標重心位置摂動量は、目標重力モーメント摂動量に比例あるいはほぼ比例して変化する。
【０１４２】
以上が、対象物反力平衡制御装置の挙動である。尚、上記制御演算は、全て、制御周期毎に実行される。従って、実対象物反力モーメントがいつ変化しても、常に、動バランスは維持される。換言すれば、対象物反力モーメントが目標値からずれたとき、ロボット１は最初は全床反力モーメントを操作してつまさき（足平２２Ｒ（Ｌ）の先端）を踏ん張るように姿勢制御されると共に、経時的に上体を前方に移動させて重力モーメントに頼るように切り換えられる。
【０１４３】
図１２はこの発明の第２の実施の形態を示し、リミッタ２００を設け、最終到達目標重心位置摂動量に上限下限の制限値を設定してリミットをかけるようにしたものである。
【０１４４】
上記した実施の形態において、実際には、最終到達目標重心位置摂動量をあまり大きくすると、ロボットの姿勢がとれなくなる場合が生じる。従って、これを防ぐために、式７によって得られた最終到達目標重心位置摂動量に、上限下限の制限値（範囲）を設定してリミットをかけるようにした。尚、その制限値（リミット値）は固定値でも良く、あるいは可変値としても良い。
【０１４５】
更に、第２のリミッタ３００を設け、モデル制御則演算器で演算された対象物反力平衡制御用補償全床反力モーメントにも上限下限の制限値（範囲）を設定し、リミットをかけるようにした。
【０１４６】
即ち、対象物反力平衡制御用補償全床反力モーメントは実ロボットの足平２２Ｒ（Ｌ）にも発生させるが、実ロボットの足平が発生できる全床反力モーメントには限度があり、限度を超えると、足平の接地性が損なわれたり、足平の一部が床から浮いたりする。それを防ぐためには、モデル制御則演算器が式１０を用いて演算した対象物反力平衡制御用補償全床反力モーメントに上限下限の制限値を設定し、リミットをかけるようにした。その制限値が固定でも可変でも良いことはリミッタ２００の場合と同様である。
【０１４７】
図１３はこの発明の第３の実施の形態を示し、リミッタ４００を設けて最終到達目標重心位置摂動量算出部の入力に上限下限の制限値を設定してリミットをかけると共に、第２のリミッタ５００を設け、第１のリミッタ４００を超えた入力値で対象物反力平衡制御用補償全床反力モーメントを修正するようにした。
【０１４８】
図１２に示した第２の実施の形態においては、最終到達目標重心位置摂動量が過大になるのを防止するために、式７によって得られた最終到達目標重心位置摂動量に、上限下限の制限値（範囲）を設定してリミットをかけるようにしたが、制限値（リミット値）を超えた値がモデル制御則演算器の出力に加算点６００（図１２）において加算され、モデルに入力されることがあった。このため、対象物反力モーメント偏差に抗して重心位置を摂動していたものが、対象物反力モーメント偏差が過大になり、リミッタが作動すると、重心位置を逆方向に揺動してしまうと言う不都合を生じることがあった。
【０１４９】
第３の実施の形態は上記した不都合を解消するもので、第１のリミッタ４００の制限値（リミット値）を超えた入力値を第２のリミッタ５００を通して加減算点７００に送り、そこでモデル制御則演算器の出力から減算、換言すれば極性を反転させて転倒させようとする力と逆向きの力を与えるように、対象物反力平衡制御用補償全床反力モーメントを修正するようにした。これによって、ロボットは制限値（リミット値）を超えた入力値（モーメント偏差）を足平２２Ｒ（Ｌ）で支持するように姿勢制御される。
【０１５０】
第２の実施の形態と異なり、リミット値を超えた値がモデル制御則演算器の出力に加算点７１０（図１３）において加算されてモデルに入力されることがない。このため、前記したような対象物反力モーメント偏差が過大になってリミッタが作動し、重心位置を逆方向に揺動してしまうと言う不都合を解消することができる。尚、残余の構成は従前の実施の形態と異ならない。第１のリミッタ４００の制限値（リミット値）が固定でも可変でも良いことも、従前の実施の形態と同様である。さらに、モデル制御則演算器の後に、リミッタ４００と同様のリミッタを追加しても良い。
【０１５１】
図１４はこの発明の第４の実施の形態を示し、対象物反力平衡制御装置において摂動動力学モデルの精度を高めるために、慣性力モーメントＩを与えた倒立振子モデルを使用するようにした。
【０１５２】
更に、摂動動力学モデルについて敷衍すると、重心高さがあまり変わらないならば、ｈは固定で良いが、作業によって重心高さが変わる場合には、最終修正目標上体位置・姿勢、最終修正目標足平位置・姿勢および最終修正目標ハンド位置・姿勢から求められるロボット姿勢から重心高さを求め、これに応じてｈを変更しても良い。
【０１５３】
また、摂動動力学モデルの精度をより高めるために、脚腕のリンクを持つロボットの多リンク幾何学モデルを備え、最終修正目標上体位置・姿勢、最終修正目標足平位置・姿勢および最終修正目標ハンド位置・姿勢から求められる重心位置と、最終修正目標上体位置・姿勢から上体位置の摂動量を差し引いた上体位置・姿勢、最終修正目標足平位置・姿勢および最終修正目標ハンド位置・姿勢から求められる重心位置との差を求めることにより、高精度な重心位置の摂動量と上体位置の摂動量との関係を求め、それを用いて重心位置の摂動量から上体位置の摂動量を求めても良い。
【０１５４】
また、脚腕のリンクを持つロボットの多リンク動力学モデルであって、運動パターンにある拘束条件を与えておいて、目標床反力の摂動を入力として目標上体位置・姿勢摂動量と重心位置・姿勢摂動量を出力させるモデルを用いても良い。
【０１５５】
ところで、腕を目標作業パターンから摂動させたときの腕の慣性力摂動量およびまたは重力摂動量の影響を考慮した摂動動力学モデルを用いると、制御装置の負荷が大きくなる。
【０１５６】
何故なら、腕の慣性力摂動量およびまたは重力摂動量は、目標上体位置・姿勢摂動量に影響され、逆に目標上体位置・姿勢摂動量は、腕の慣性力摂動量およびまたは重力摂動量に影響されるので、この相互作用を同時に考慮してモデル挙動を算出することは、非常に複雑な演算が必要となるからである。
【０１５７】
その問題を解決する手段として、以下の手法を用いても良い。
【０１５８】
即ち、摂動動力学モデルでは、腕を目標作業パターンから摂動させたときの腕の慣性力摂動量およびまたは重力摂動量の影響を無視し、腕は目標動作パターン通りにしか動かないものと仮定する。この仮定により、モデルは摂動動力学モデルの詳細説明に例として挙げた倒立振子モデルと同一形式に近似される。従って、摂動動力学モデルの演算は極めて簡単になる。
【０１５９】
腕メイン制御装置において、目標ハンド位置・姿勢、目標上体位置、最終修正目標ハンド位置・姿勢および最終修正目標上体位置から、腕を目標姿勢から最終目標姿勢に摂動させたために生じた慣性力摂動量およびまたは重力摂動量を算出する。これは、従来からの手法である多リンクマニピュレータの動力学演算を行うことにより得られる。これは、最終修正目標ハンド位置・姿勢座標系で表す。
【０１６０】
算出された慣性力摂動量およびまたは重力摂動量を、センサによって検出された実対象物反力に加え、対象物反力平衡制御装置に実対象物反力として出力する。以上の手段により、腕の慣性力摂動量およびまたは重力摂動量の影響を、摂動動力学モデルにおいて無視した代わりに、作業対象物反力として考慮したこととなる。腕の慣性力摂動量およびまたは重力摂動量の演算と摂動動力学モデルの演算が独立して行われるので、複雑な干渉演算が不要となり、演算量が小さくて済む。
【０１６１】
上記の如く、第１ないし第４の実施の形態にあっては、少なくとも基体（上体２８）と、前記基体に連結される複数本のリンク（脚リンク２および腕リンク３）からなる脚式移動ロボット（２足歩行ロボット１）の姿勢制御装置において、前記ロボットの少なくとも前記基体の目標軌跡を含む運動パターンと、前記ロボットに作用する床反力の目標軌跡と、前記ロボットに作用する床反力以外の外力の目標軌跡とを少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段（目標作業パターン生成器）、前記床反力以外の外力を検出する外力検出手段（６軸力センサ５８）、前記検出された外力と、前記目標軌跡で設定された床反力以外の外力の偏差（目標全床反力中心点まわりの対象物反力モーメント偏差）を演算する外力偏差演算手段（対象物反力平衡制御装置。より具体的には、図８の実対象物反力の座標変換およびその入出力）、前記床反力の摂動と前記ロボットの重心位置および／または基体の位置の摂動の関係を表現するモデル（摂動動力学モデル）、少なくとも前記演算された外力の偏差に基づいて前記モデルに入力すべきモデル入力量（モデルのための目標全床反力モーメント摂動量）を演算するモデル入力量演算手段（モデル制御則演算器およびその後の加算点での入出力）、前記演算されたモデル入力量を前記モデルに入力し、得られる前記重心位置および／または基体の摂動量に応じて前記基体の目標軌跡を修正する、基体目標軌跡修正量（修正目標上体位置・姿勢）を演算する基体目標軌跡修正量演算手段（対象物反力平衡制御装置。より具体的には、摂動動力学モデル入力量を入力し、モデルの挙動を演算し、モデル出力から目標上体位置姿勢摂動量（修正量）を求める部分）、少なくとも前記演算されたモデル入力量に応じて前記床反力の目標軌跡を修正する、床反力目標軌跡修正量（対象物反力平衡制御用補償全床反力モーメント）を演算する床反力目標軌跡修正量演算手段（モデル制御則演算器、より具体的にはモデル制御則の一部）、および少なくとも前記演算された基体目標軌跡修正量および床反力目標軌跡修正量に基づいて前記ロボットの関節を変位させる関節変位手段（脚メイン制御装置、アクチュエータ駆動装置８８、脚アクチュエータなど）、を備える如く構成した。
【０１６２】
また、前記モデル入力量演算手段は、前記外力に静的に平衡する平衡重心位置の摂動量を算出する平衡重心位置摂動量算出手段（最終到達目標重心位置摂動量算出部）、を備え、前記算出された平衡重心位置に前記モデルが収束するように前記モデル入力量を演算する如く構成した。
【０１６３】
また、前記モデルが前記ロボットを倒立振子で近似するモデル（摂動動力学モデル）である如く構成した。
【０１６４】
また、前記平衡重心位置摂動量算出手段は、前記算出された平衡重心位置の摂動量を所定の範囲に制限するリミッタ２００，４００を備える如く構成した。
【０１６５】
また、前記床反力目標軌跡修正量演算手段は、前記演算された床反力目標軌跡修正量を所定の範囲に制限するリミッタ３００，５００を備えるように構成した。
【０１６６】
また、前記床反力の目標軌跡は、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む如く構成した。
【０１６７】
また、前記床反力目標軌跡修正量演算手段は、前記床反力目標軌跡修正量（対象物反力平衡制御補償用全床反力モーメント）が、前記モデル入力量（モデルのための目標全床反力モーメント摂動量）から前記外力の偏差（目標全床反力中心点まわりの対象物反力モーメント偏差）を減算した値と、前記床反力の目標中心点まわりに作用するモーメントに動力学的に釣り合うように、前記床反力目標軌跡修正量を演算する如く構成した。
【０１６８】
また、前記床反力以外の外力が、前記リンクを介して前記ロボットに作用する作業対象物（台車１００）からの反力である如く構成した。
【０１６９】
また、前記ロボットが、前記基体に連結される２本の脚リンク２と２本の腕リンク３からなる脚式移動ロボットである如く構成した。
【０１７０】
また、少なくとも基体（上体２８）と、前記基体に連結される複数本のリンク（脚リンク２、腕リンク３）からなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンと、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段（目標作業パターン生成器）、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段（６軸力センサ５８）、前記検出された対象物反力を前記目標床反力中心点まわりのモーメントとして変換する対象物反力モーメント変換手段（対象物反力平衡制御装置）、前記変換された対象物反力モーメントに動力学的に釣り合うように、前記目標中心点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段（対象物反力平衡制御装置）、および前記修正された目標中心点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段（脚メイン制御装置、アクチュエータ駆動装置８８、脚アクチュエータなど）、を備える如く構成した。
【０１７１】
また、少なくとも基体（上体２８）と、前記基体に連結される複数本のリンク（脚リンク２、腕リンク３）からなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンを設定する目標歩容設定手段（目標作業パターン生成器）、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段（６軸力センサ５８）、前記検出された対象物反力を所定の点、より具体的には目標床反力中心点まわりのモーメントとして変換する対象物反力モーメント変換手段（対象物反力平衡制御装置）、前記変換された対象物反力モーメントに動力学的につりあうように、前記所定の点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段（対象物反力平衡制御装置）、および前記修正された所定の点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段（脚メイン制御装置、アクチュエータ駆動装置８８、脚アクチュエータなど）、を備える如く構成した。
【０１７２】
尚、上記した第１ないし第４の実施の形態においては、上体リンクの曲げやひねりのためのアクチュエータを設けなかったが、それを追加するとき、上体アクチュエータ制御装置も必要となる。但し、上体リンクの曲げやひねりは腕または脚の付け根側に関節を追加したことと等価であるので、概念上、腕または脚のアクチュエータとみなすことができる。即ち、上体アクチュエータ制御装置は、腕または脚の制御装置の一部として含まれると考えることができる。
【０１７３】
上記した第１ないし第４の実施の形態では、先に特開平５−３０５５８６号公報で提案したコンプライアンス制御を用いているが、それ以外の手段を用いても良い。脚制御に、そのコンプライアンス制御以外の別の手段、たとえば、電動アクチュエータを電流指令型のアンプによって制御する手段を用いて関節トルクを制御し、その結果、間接的に床反力を制御する手段を用いれば、足平２２Ｒ（Ｌ）に設けた６軸力センサ５６は不要である。
【０１７４】
更に、上記した第１ないし第４の実施の形態において、腕の制御に、仮想コンプライアンス制御以外の別の手段、たとえば、電動アクチュエータを電流指令型のアンプによって制御する手段を用いて関節トルクを制御し、その結果、間接的に対象物反力を制御しても良い。その制御ではハンドの６軸力センサは不要であるが、対象物反力平衡制御装置のために、ハンドの６軸力センサは設けるのが良い。
【０１７５】
更に、上記した第１ないし第４の実施の形態において、ハンドの６軸力センサの代わりに、関節トルクから実対象物反力を推定する推定器を腕制御装置に備えても良い。この推定器は、従来技術である外乱オブザーバーを用いれば良い。
【０１７６】
更には、上記した第１ないし第４の実施の形態において、特開平５−３０５５８６号で提案したコンプライアンス制御に加えて、本出願人が特開平５−３３７８４９号公報で提案した制御を加えても良い。但し、その制御によって上体の位置や歩幅が修正されるので、腕制御においてハンドと作業対象物との相対位置関係が重要な場合には、その制御によって修正される上体の位置や歩幅の影響を考慮する必要がある。
【０１７７】
更には、上記した第１ないし第４の実施の形態において、床が平面でない場合でも、本出願人が特開平５−３１８８４０号公報で提案した仮想平面を想定する技術を用いて目標全床反力中心点や目標ＺＭＰを仮想平面上に求めても良い。
【０１７８】
更には、上記した第１ないし第４の実施の形態において、ロボット全体の姿勢が目標からずれて傾くと、ハンドの位置・姿勢が絶対空間においてずれる。この結果、対象物反力が目標対象物反力から大きくずれる場合がある。
【０１７９】
その問題点を解決するために、傾斜センサによって検出される実上体位置・姿勢と目標上体位置・姿勢のずれに応じて上記の修正された最終目標ハンド位置・姿勢をさらに補正することにより、ロボット全体の姿勢が傾いても、ハンドの位置・姿勢が絶対空間においてずれないようにするのが、より好ましい。
【０１８０】
更には、上記した第１ないし第４の実施の形態において、ブロック図は演算処理順序を変えるなど、種々の変形が可能である。
【０１８１】
更には、上記した第１ないし第４の実施の形態においてはＰＤ制御則を用いたが、それ以外の制御則（たとえば、PID制御、状態フィードバック制御）などを用いても良い。
【０１８２】
また、この発明を腕を備えた２足歩行の脚式移動ロボットについて説明したが、腕を備えない脚式移動ロボットにも有益であり、更に２足歩行ロボットに限らず、多脚ロボットにも応用することができる。
【０１８３】
【発明の効果】
脚式移動ロボットが予期できない外力、より具体的には作業対象物から反力を受けても、動バランスをとって安定な姿勢を継続することができる。更に、予め想定していなかった動作パターンでリンク、より具体的には腕を動かして作業をするときに腕に発生する重力、慣性力だけでなく、作業対象から予期せぬ反作用を受けても、動バランスをとって安定な姿勢を継続することができる。
【０１８４】
また、対象物反力が急変するときも、それに静的にバランスをとる位置にロボットの重心を移動させることにより、傾きや転倒を効果的に抑制することができる。また、ロボットの重心が移動する過渡期においても、重心位置や床反力を適正に変化させて動バランスを維持し続けることができる。
【図面の簡単な説明】
【図１】この発明に係る脚式移動ロボットの姿勢制御装置を全体的に示す説明図である。
【図２】図１に示す２足歩行ロボットの制御ユニットの詳細を示すブロック図である。
【図３】この発明に係る脚式移動ロボットの姿勢制御装置の構成および動作を機能的に示すブロック図である。
【図４】図１に示す脚式移動ロボットが腕を使用して行う作業を示す説明図である。
【図５】図３装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。
【図６】図５と同様に、図３装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。
【図７】図３に示す対象物反力平衡制御装置の動作を説明するタイミング・チャートである。
【図８】図３に示す対象物反力平衡制御装置の詳細な構成を示すブロック図の前半部である。
【図９】図３に示す対象物反力平衡制御装置の詳細な構成を示すブロック図の後半部である。
【図１０】図９に示す対象物反力平衡制御装置の摂動動力学モデルを示す説明図である。
【図１１】図１０に示すモデルを倒立振子で近似した状態を示す説明図である。
【図１２】図９に類似する、この発明の第２の実施の形態を示す対象物反力平衡制御装置の詳細な構成を示すブロック図の後半部である。
【図１３】図９に類似する、この発明の第３の実施の形態を示す対象物反力平衡制御装置の詳細な構成を示すブロック図の後半部である。
【図１４】図１１に類似する、この発明の第４の実施の形態を示す倒立振子型摂動動力学モデルを示す説明図である。
【符号の説明】
１２足歩行ロボット（脚式移動ロボット）
２脚リンク
３腕リンク
１０，１２，１４Ｒ，Ｌ腰関節
１６Ｒ，Ｌ膝関節
１８，２０Ｒ，Ｌ足関節
２２Ｒ，Ｌ足平
２８上体
３０，３２，３４Ｒ，Ｌ肩関節
３６Ｒ，Ｌ肘関節
３８，４０，４２Ｒ，Ｌ手首関節
４４Ｒ，Ｌハンド
５０制御ユニット
５６，５８６軸力センサ
６０傾斜センサ[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a posture control device for a legged mobile robot, and more particularly to a legged mobile robot, and more particularly a bipedal legged mobile robot that takes a dynamic balance even when subjected to an unexpected object reaction force. In particular, the application of the leg-type mobile robot equipped with arms as a leg-arm cooperative control is effective.
[0002]
In this specification, “object reaction force” is an external force received from the environment including the work target, and is used to indicate a floor reaction force that acts on the robot from the ground contact surface.
[0003]
[Prior art]
As a legged mobile robot, especially a bipedal legged mobile robot equipped with arms, “Development of a biped walking robot that compensates for a three-axis moment by upper body motion” (Vol. 11 of Vol. 11 of the Robotics Society of Japan) No., May 1993). This robot is provided with a pendulum as a simplified arm, and a desired gait including gravity and inertia generated by swinging the robot is designed in advance, and walking control is performed so as to follow it.
[0004]
[Problems to be solved by the invention]
However, in this prior art, it is assumed that the arm does not receive the object reaction force. Therefore, when the proposed control is applied not only to walking but also to work, if an unexpected reaction is received from the work object, the dynamic balance is lost and the posture becomes unstable, or in the worst case falls There was a fear.
[0005]
Further, the present applicant has also proposed a legged mobile robot of the same kind in Japanese Patent Application Laid-Open No. 7-205069. In this case, when the frictional force decreases during walking, the arm is swung to recover a stable posture. ing.
[0006]
However, in the legged mobile robot proposed by the present applicant, the legs and arms are independently and independently controlled without cooperation, so that when the arms are driven, the gravity and inertial force generated by the arms, and the work In some cases, the reaction balance from the subject disrupts the dynamic balance of the entire robot, making the posture of the robot unstable.
[0007]
Accordingly, an object of the present invention is to eliminate the above-mentioned inconveniences, and even if the legged mobile robot receives an unexpected object reaction force, the legged movement can maintain a stable posture by keeping a dynamic balance. The object is to provide a robot posture control device.
[0008]
The second object of the present invention is to provide a leg type that effectively suppresses tilting and overturning by moving the center of gravity of the robot to a position where it statically balances even when the reaction force of the object suddenly changes. An object is to provide a posture control device for a mobile robot.
[0009]
The third object of the present invention is to maintain the dynamic balance by appropriately changing the position of the center of gravity and the floor reaction force even in the transition period when the center of gravity of the robot moves when receiving the above-described object reaction force. An object of the present invention is to provide an attitude control device for a legged mobile robot.
[0010]
The fourth object of the present invention is not only gravitational force and inertial force generated in the arm when working by moving the arm in a motion pattern that was not assumed in advance, in a legged mobile robot having an arm, An object of the present invention is to provide a posture control device for a legged mobile robot that can maintain a stable posture even when it receives an unexpected reaction from a work target.
[0011]
[Means for Solving the Problems]
In order to achieve the above object, according to claim 1, in a posture control apparatus for a legged mobile robot comprising at least a base and a plurality of links connected to the base, at least the base of the robot A target gait of the robot including at least a motion pattern including a target locus of the robot, a target locus of a floor reaction force acting on the robot, and a target locus of an external force other than the floor reaction force acting on the robot. Target gait setting means, external force detection means for detecting an external force other than the floor reaction force, the detected external force, and an external force other than the floor reaction force set by the target locus When External force deviation calculating means for calculating the deviation of the robot, perturbation of the floor reaction force, the center of gravity position of the robot and Base Body position At least one of A model that expresses the perturbation relationship of the model, a model input amount calculation means that calculates a model input amount to be input to the model based on at least the calculated deviation of the external force, and inputs the calculated model input amount to the model The obtained center of gravity position and Base body At least one of A base target trajectory correction amount calculating means for calculating a base target trajectory correction amount that corrects the target trajectory of the base body in accordance with a perturbation amount of the base, and a target trajectory of the floor reaction force in accordance with at least the calculated model input amount. The floor reaction force target locus correction amount calculating means for calculating the floor reaction force target locus correction amount to be corrected, and the joint of the robot based on at least the calculated base body target locus correction amount and floor reaction force target locus correction amount. And a joint displacing means for displacing.
[0012]
Here, “position” is used to mean “position and / or posture” except for the position of the center of gravity. Note that “posture” means a direction in a three-dimensional space as described later.
[0013]
Here, the “target reaction locus of the floor reaction force” is more specifically used to mean that it includes at least the target locus of the floor reaction force center point. More specifically, “correcting the target locus of the floor reaction force” is used to mean correcting the moment around the center point of the floor reaction force.
[0014]
Here, “detecting an external force” is used to mean not only detection but also estimation using a disturbance observer or the like.
[0015]
According to claim 2, the model input amount calculation means includes equilibrium centroid position perturbation amount calculation means for calculating a perturbation amount of an equilibrium centroid position that is statically balanced with the external force. The model input amount is calculated so that the model converges at the center of gravity.
[0016]
According to a third aspect of the present invention, the model is configured to approximate the robot with an inverted pendulum.
[0017]
According to a fourth aspect of the present invention, the balanced center-of-gravity position perturbation amount calculating means includes a limiter that limits the calculated perturbation amount of the balanced center-of-gravity position to a predetermined range.
[0018]
According to a fifth aspect of the present invention, the floor reaction force target trajectory correction amount calculating means is configured to include a limiter that limits the calculated floor reaction force target trajectory correction amount to a predetermined range.
[0019]
According to a sixth aspect of the present invention, the target locus of the floor reaction force is configured to include at least the locus of the target center point of the floor reaction force acting on the robot.
[0020]
The floor reaction force target trajectory correction amount calculating means according to claim 7, wherein the floor reaction force target trajectory correction amount calculating unit subtracts a deviation of the external force from the model input amount, and the floor reaction force target trajectory correction amount. The floor reaction force target locus correction amount is calculated so as to dynamically balance the moment acting around the target center point.
[0021]
According to an eighth aspect of the present invention, the external force other than the floor reaction force is a reaction force from a work object acting on the robot via the link.
[0022]
In the ninth aspect, the robot is configured as a legged mobile robot including two leg links and two arm links connected to the base body.
[0023]
In the posture control device for a legged mobile robot comprising at least a base and a plurality of links connected to the base, a movement pattern including at least a target position of the base of the robot, A target gait setting means for setting a target gait of the robot including at least a locus of a target center point of a floor reaction force acting on the robot; a reaction from a work object acting on the robot via the link; An object reaction force detecting means for detecting a force, and the detected object reaction force is detected by the eye Mark An object reaction force moment conversion means for converting the moment around the center point, a floor reaction force moment about the target center point, a position of the robot, and a robot so as to dynamically balance the converted object reaction force moment; A robot position / posture correction unit that corrects the posture, and a joint displacement unit that displaces the joint of the robot based on the floor reaction force moment around the corrected target center point and the position / posture of the robot. did.
[0024]
12. The posture control apparatus for a legged mobile robot comprising at least a base body and a plurality of links connected to the base body, wherein the movement pattern including at least the target position of the base body is set. Target gait setting means, object reaction force detection means for detecting a reaction force from a work object acting on the robot via the link, and a moment around a predetermined point for the detected object reaction force An object reaction force moment conversion means for converting the object reaction force moment, and a robot for correcting the floor reaction force moment about the predetermined point and the position and posture of the robot so as to dynamically balance the converted object reaction force moment Position / posture correction means, and the robot based on the corrected floor reaction force moment around the predetermined point and the position / posture of the robot Joint displacement means for displacing the joint was composed as comprising a.
[0025]
In the above description, the “legged mobile robot” includes a legged mobile robot that receives an object reaction force in addition to the arm. Further, regarding the “arm link”, even a leg link is regarded as an arm link if it acts on a work target. For example, in an insect-type six-legged robot, when an object is lifted using the previous two legs, the leg link is regarded as an arm link.
[0026]
[Action]
According to the first aspect of the present invention, even when the legged mobile robot receives an unexpected external force, more specifically, a reaction force from the work object, a stable posture can be maintained with a dynamic balance. Furthermore, the link with an unforeseen movement pattern, more specifically, not only the gravity and inertial force generated in the arm when moving the arm, but also the unexpected reaction from the work target , You can keep a stable posture with dynamic balance.
[0027]
Also, when the reaction force of an object suddenly changes, And By moving the center of gravity of the robot to a position where the robot is tilted, tilting and falling can be effectively suppressed. Further, even in a transition period in which the center of gravity of the robot moves, it is possible to maintain the dynamic balance by appropriately changing the position of the center of gravity and the floor reaction force.
[0028]
Claims 2 to 8 have the same functions and effects as those of Claim 1.
[0029]
In the ninth aspect, in addition to the functions and effects described above, even when the arm is a legged mobile robot, the arm is used when the arm is moved in a motion pattern that has not been assumed in advance. Not only the generated gravity and inertial force but also an unexpected reaction from the work object, it is possible to maintain a stable posture with a dynamic balance.
[0030]
The tenth to eleventh aspects have the same operations and effects as the first aspect.
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, a legged mobile robot posture control apparatus according to the present invention will be described with reference to the accompanying drawings. As a legged mobile robot, a biped walking robot is taken as an example.
[0031]
FIG. 1 is a schematic diagram showing the posture control apparatus of the legged mobile robot as a whole.
[0032]
As shown in the figure, the biped robot 1 includes six joints on the left and right leg links 2 (for convenience of understanding, each joint is indicated by an electric motor that drives it).
[0033]
The six joints are, in order from the top, joints 10R and 10L for leg rotation at the waist (R on the right side and L on the left side, the same shall apply hereinafter),

joints

12R and 12L on the roll axis (around the Y axis) of the waist, It consists of joints 14R, 14L on the pitch axis (around the X axis), joints 16R, 16L on the roll axis of the knee, joints 18R, 18L in the roll direction of the foot, and joints 20R, 20L on the same pitch axis.

Foot feet

22R and 22L are attached to the foot portions.
[0034]
In the above, the hip joint (or waist joint) is composed of joints 10R (L), 12R (L), 14R (L), and the ankle joint is composed of joints 18R (L), 20R (L). The hip joint and knee joint are connected by thigh links 24R and 24L, and the knee joint and ankle joint are connected by crus links 26R and 26L.
[0035]
Further, an upper body (or base body, indicated by a link) 28 is provided above the waist, and an upper end thereof is provided with arm links 3 including seven joints on the left and right sides (also for convenience of understanding). Each joint is represented by an electric motor that drives it).
[0036]
The seven joints are, in order from the top, shoulder

roll axis joints

30R and 30L, pitch axis joints 32R and 32L,

arm rotation joints

34R and 34L, elbow roll axis joints 36R and 36L, It comprises

wrist joints

38R and 38L,

joints

40R and 40L with the same roll axis, and

joints

42R and 42L with the same pitch axis. Hands (end effectors) 44R and 44L are attached to the tip of the wrist.
[0037]
In the above, the shoulder joint is composed of joints 30R (L), 32R (L), 34R (L), and the wrist joint is composed of joints 38R (L), 40R (L), 42R (L). The shoulder joint and elbow joint are connected by upper arm links 46R and 46L, and the elbow joint and wrist joint are connected by

lower arm links

48R and 48L.
[0038]
Note that a control unit 50 including a microcomputer, which will be described later with reference to FIG.
[0039]
With the above configuration, the leg link 2 is provided with six degrees of freedom for the left and right feet, and by driving these 6 * 2 = 12 joints at an appropriate angle during walking, the entire leg has a desired degree of freedom. Movement can be given and can be arbitrarily walked in a three-dimensional space ("*" indicates multiplication in this specification). As described above, the moving direction (pitch axis) of the robot is the X axis, the left and right direction (roll axis) is the Y axis, and the vertical direction (gravity axis) is the Z axis.
[0040]
Also, the arm link 3 is given seven degrees of freedom for the left and right arms, and by driving these 7 * 2 = 14 joints at an appropriate angle, a desired operation such as pushing a carriage described later is performed. It can be carried out.
[0041]
As shown in FIG. 1, a known 6-axis force sensor 56 is attached to the foot 22R (L) below the ankle joint, and among the external forces that act on the robot, the floor reaction force that acts on the robot from the ground contact surface. The three-direction components Fx, Fy, Fz and the three-direction components Mx, My, Mz of the moment are detected.
[0042]
Furthermore, a 6-axis force sensor 58 of the same kind is attached between the wrist joint and the hand 44R (L), and the other external forces acting on the robot, particularly the above-described object reaction force received from the work object, are provided in three directions. The components Fx, Fy, Fz and the three-direction components Mx, My, Mz of the moment are detected.
[0043]
In addition, an inclination sensor 60 is installed on the body 28 to detect the inclination with respect to the Z axis (vertical axis (gravity axis)) and its angular velocity. The electric motor of each joint is provided with a rotary encoder for detecting the amount of rotation while relatively displacing the links 24, 26R (L) and the like via a speed reducer (not shown) that decelerates and increases the output. Thus, it is configured as a leg actuator or arm actuator with a displacement detector. The outputs of these six-axis force sensors 56 and the like are sent to the control unit 50 (only the right side of the robot 1 is shown for convenience of illustration).
[0044]
FIG. 2 is a block diagram showing the details of the control unit 50, which is composed of a microcomputer. The output from the tilt sensor 60 and the like is converted into a digital value by the A / D converter 70, and the output is sent to the RAM 74 via the bus 72. In addition, the output of the encoder disposed adjacent to the electric motor in each actuator is input into the RAM 74 via the counter 76.
[0045]
In the control unit, an arithmetic unit 80 comprising a CPU is provided. As will be described later, the arithmetic unit 80 allows the robot to continue a stable posture based on the gait stored in the ROM 84. A joint angle displacement command (actuator displacement command) is calculated and sent to the RAM 74.
[0046]
The arithmetic unit 80 reads out the command and the detected actual value from the RAM 74, calculates a control value (operation amount) necessary for driving each joint, and calculates the D / A converter 86 and the actuator provided in each joint. It outputs to the electric motor of the leg actuator which drives each joint via the drive device (amplifier) 88, and an arm actuator.
[0047]
FIG. 3 is a block diagram functionally showing the configuration and operation of the posture control device (mainly equivalent to the above-described arithmetic device 80) of the legged mobile robot according to the present invention.
[0048]
This device is a device that controls the movement of the legs and arms in an integrated manner, and outputs a displacement command to each actuator driving device 88. As shown in the figure, this device is composed of a target work pattern generator, an object reaction force balance control device, a leg main control device, and an arm main control device.
[0049]
In the following, for convenience of understanding, the processing contents of each component of this apparatus will be described by taking the robot work situation shown in FIG. 4 as an example. In FIG. 4, when the robot 1 is pushing the carriage 100, the absolute value of the actual object reaction force received from the carriage suddenly becomes smaller than the target object reaction force assumed in the target work pattern. As a result, the robot 1 loses its balance and leans forward. The apparatus according to this embodiment controls to always maintain a dynamic balance even in such a situation.
[0050]
The target work pattern generator generates a target work pattern that satisfies a dynamic equilibrium condition under a certain assumed condition. The target work pattern is expressed by a time change pattern of a plurality of variables. This variable is composed of a variable expressing a motion and a variable expressing a reaction force received from the environment.
[0051]
Here, the variable expressing the motion is a set of variables that can uniquely determine the posture at each moment. Specifically, it is composed of a target foot position / posture, a target body position / posture, and a target hand position / posture.
[0052]
The variable expressing the reaction force received from the environment is composed of the target total floor reaction force central point (position) (target ZMP (position)), the target total floor reaction force, and the target object reaction force.
[0053]
Each of these variables is expressed in the support leg coordinate system. The support leg coordinate system is a coordinate system whose origin is a vertical projection point from the support leg ankle (intersection of the joints 18 and 20R (L)) to the foot 22R (L), as shown in FIGS. The coordinate system is fixed to the floor with which the support legs are in contact, with the support leg foot facing forward in the X-axis direction, leftward in the Y-axis direction, and vertically upward in the Z-axis direction. .
[0054]
Details of each of these variables will be described below.
[0055]
As described above, when the external force excluding each foot floor reaction force among the external forces received by the robot from the environment is called an object reaction force, the target object reaction force is the target value. In the example of FIG. 4, this is a reaction force that the hand 44 </ b> R (L) receives from the object 100.
[0056]
The target object reaction force output by the target work pattern generator is expressed by a force and a moment acting around a target total floor reaction force center point described later. Incidentally, the moment component is important for posture stabilization.
[0057]
The target total floor reaction force and the target total floor reaction force center point (position) will be described. The resultant force of the desired floor reaction force that each foot should receive from the floor during work is referred to as a target total floor reaction force in a broad sense. The target total floor reaction force in a broad sense is expressed by the target total floor reaction force center point and the force and moment at that point. The target total floor reaction force central point is the point on the floor where the moment component around the X axis and the moment component around the Y axis are zero when the target total floor reaction force is expressed as a force and moment with that point as the point of action. It is.
[0058]
The target total floor reaction force in a narrow sense means a force and a moment when the target total floor reaction force in a broad sense is expressed as a force and a moment with the target total floor reaction force central point as an action point. The target total floor reaction force output by the target work pattern generator is a target total floor reaction force in a narrow sense.
[0059]
Hereinafter, unless otherwise specified, the target total floor reaction force refers to the target total floor reaction force in a narrow sense. When walking on a flat floor surface, the point of action of the desired total floor reaction force is normally set on the floor surface.
[0060]
The concept of ZMP conventionally known in the field of walking control also extends the concept as follows. That is, when the resultant force of the inertial force, gravity, and object reaction force generated by the movement of the robot is expressed by a force and a moment with that point as the action point, the moment component around the X axis and the moment component around the Y axis are zero. A point on the floor is called ZMP. The ZMP when the robot performs the target motion is called a target ZMP (position).
[0061]
Saying that the target work pattern satisfies the dynamic equilibrium condition means that the above-described inertial force, gravity and the resultant force of the object reaction force and the target total floor reaction force generated by the target work pattern cancel each other and become zero. is there. Therefore, in order to satisfy the dynamic equilibrium condition, the target total floor reaction force central point and the target ZMP must match.
[0062]
The target work pattern generator generates a target work pattern that satisfies the dynamic equilibrium condition. Therefore, the target total floor reaction force central point (position) generated by the target work pattern generator coincides with the target ZMP (position).
[0063]
The target foot position / posture, the target body position / posture, and the target hand position / posture represent the positions and postures of the respective parts expressed in the support leg coordinate system. Specifically, in this specification, the position of the upper body 28 and its speed mean representative points such as the position of the center of gravity of the upper body 28 and its (displacement) speed. Further, the posture of the upper body or the foot means “orientation” in the X, Y, Z space.
[0064]
The object reaction force balance control device and its control form the center of the control of this embodiment, and the object reaction force balance control device performs control while taking into account the dynamic balance condition in order to balance the posture. . Therefore, before describing the outline of the object reaction force balance control apparatus, the dynamic balance conditions will be described below.
[0065]
The biggest factor that determines the behavior of the actual robot posture inclination is the balance of the moment of the actual force around the target total floor reaction force center point (ie, the target ZMP).
[0066]
The moments of the force acting around the target total floor reaction force center point are listed below.
1) Moment of inertia force
2) Gravitational moment
3) Total floor reaction force moment
4) Object reaction force moment
[0067]
The above moments have been described above, but are defined again below.
[0068]
The inertial force moment is a moment generated by a change in the angular momentum of the robot around the target total floor reaction force center point. This value is obtained by the Euler equation. Specifically, it is obtained by inverting the sign of the first-order differential value of the angular momentum of the robot around the target total floor reaction force center point.
[0069]
The inertial moment of force in the target work pattern is called a target inertial moment. The moment of inertia force when the actual robot is working is called the actual moment of inertia force.
[0070]
The gravitational moment is a moment in which gravity acting on the center of gravity of the robot acts around the center point of the target total floor reaction force.
[0071]
The resultant force of the floor reaction force acting on each foot is called the total floor reaction force. The total floor reaction force moment is a moment at which the total floor reaction force acts around the target total floor reaction force center point.
[0072]
The reaction force received from the work object is called the object reaction force. The object reaction force moment is a moment at which the work object reaction force acts around the center point of the desired total floor reaction force.
[0073]
Now, it is assumed that the robot 1 has faithfully followed the motion pattern of the target work pattern by the ideal leg main control device. At this time, the actual inertial force moment matches the target inertial force moment, and the actual gravity moment matches the target gravity moment.
[0074]
On the other hand, the sum of the actual inertial moment, the actual gravity moment, the actual total floor reaction force moment, and the actual object reaction force moment is always 0 according to the law of dynamics (Euler equation).
[0075]
Therefore, in order for the robot 1 to faithfully move according to the motion pattern of the target work pattern, the sum of the target inertial force moment, target gravity moment, actual total floor reaction force moment, and actual object reaction force moment must be zero. . This is defined as condition 1.
[0076]
However, actually, the actual object reaction force moment does not coincide with the target object reaction force moment, and a difference occurs. For example, as described with reference to FIG. 4, the absolute value of the actual rolling friction force of the cart (ie, the target object) suddenly becomes smaller than the assumed value when the cart is pushed. Is the situation.
[0077]
In this situation, the moment that the actual object reaction force acts around the Y axis of the target total floor reaction force central point is the moment that the target object reaction force acts around the Y axis of the target total floor reaction force central point. Becomes larger in the positive direction and does not satisfy the condition 1, and the robot 1 tilts forward. As for the direction of the moment, the moment that turns the robot 1 clockwise in the positive direction of the coordinate axis is positive.
[0078]
In order to satisfy the condition 1 even in such a situation, the following two methods can be considered.
[0079]
Method 1) The actual total floor reaction force moment is changed so as to cancel the deviation. Specifically, the leg main control device is instructed to generate a negative floor reaction force moment around the target total floor reaction force center point, and the leg main control device receives this command and receives the foot 22R (L ) And increase the actual total floor reaction force moment in the negative direction. In other words, a posture that is stomped with a foot is taken.
[0080]
Method 2) The target moment of inertia force and the target gravity moment are corrected by correcting the motion pattern of the target work pattern so as to cancel the deviation. Specifically, the target inertial force moment and the target gravity moment are corrected by correcting the target body position and / or posture. That is, the upper body is moved forward.
[0081]
In the apparatus according to this embodiment, both methods are performed at the same time, and in the short term, the method 1 is mainly used to cope with a rapid change, and in the long term, the method 2 is mainly used to realize the actual total floor reaction force. The dynamic balance was always maintained while converging the moment to the original desired total floor reaction force moment.
[0082]
Since the actual total floor reaction force moment can be quickly changed by the leg main control device only by changing the target total floor reaction force moment, Method 1 is suitable for a short-term response. However, if the actual total floor reaction force moment is changed greatly, the contact pressure distribution of the foot 22R (L) is biased and the feeling of contact decreases, and in the worst case, a part of the foot 22R (L) is lifted. End up. Therefore, in the long term, the original desired total floor reaction force moment should be restored as much as possible.
[0083]
To return the actual total floor reaction force moment to the original target total floor reaction force moment, the motion pattern of the target work pattern is corrected by Method 2 so that the position of the center of gravity is shifted and the deviation is canceled by the target gravity moment. good. However, if the position of the center of gravity is suddenly shifted, an excessive target inertial force moment is generated in the opposite direction, so that it is necessary to shift the position of the center of gravity slowly. Therefore, Method 2 is suitable for a long-term response.
[0084]
The object reaction force balance control device will be described based on the above. The object reaction force balance control device is a device having the control function described above.
[0085]
The input of the object reaction force balance control device includes the target body position / posture, the target total floor reaction force center point (position), the target object reaction force, the detected value of the six-axis force sensor 58, the final corrected target hand position, Posture, final corrected target body position / posture, final corrected target foot position / posture (if using approximate calculation, final corrected target hand position / posture, final corrected target body position / posture, final correction No target foot position / posture is required).
[0086]
In the object reaction force balance control device, in order to realize the above control function, the target object reaction force is replaced with the detected value of the actual object reaction force, and the target body position / Correct posture and target total floor reaction force. As a result, the object reaction force assumed by the modified work pattern (ie, the modified target object reaction force) matches the actual object reaction force, and the dynamic equilibrium condition of the robot is satisfied.
[0087]
The outputs of the object reaction force balance control device are the corrected target body position / posture and the compensation total floor reaction force for object reaction force balance control.
[0088]
The corrected target body position / posture is the target body position / posture corrected by the object reaction force balance control device. The compensated total floor reaction force for object reaction force balance control is a total floor reaction force applied to the target total floor reaction force center point (position) by correction. Of the components of the compensation total floor reaction force for the object reaction force balance control, particularly important components for posture stabilization are an X-axis moment component and a Y-axis moment component.
[0089]
To describe only the output behavior of the object reaction force balance control device, the deviation between the actual object reaction force and the target object reaction force, more precisely, the deviation between both moments changes suddenly, that is, as shown in FIG. When changing stepwise, the moment component of the compensating total floor reaction force for object reaction force balance control responds quickly in response to this difference in order to satisfy the dynamics equilibrium condition.
[0090]
After a while, the corrected target body position / posture settles to a position / posture that statically balances this deviation, and the moment component of the total floor reaction force for object reaction force balance control converges to zero. The configuration and algorithm description of the object reaction force balance control device will be described later.
[0091]
In FIG. 3, the target values input to the leg main control device are the corrected target body position / posture, the target foot position / posture, the target total floor reaction force central point (position) and the target total floor acting on that point. It is the compensation total floor reaction force for reaction force and object reaction force balance control.
[0092]
The functions of the leg main control device can be simply described as follows. A leg actuator (an electric motor and an encoder such as the joint 10R (L)) is operated to follow a target posture and follow a target floor reaction force. It is a device that performs floor reaction force control simultaneously. Since it is impossible to completely satisfy the target posture and the target floor reaction force at the same time, appropriate adjustment is performed and control is performed so as to satisfy both in the long term.
[0093]
More specifically, in order to restore the actual body position / posture detected by the tilt sensor 60 to the corrected target body position / posture, a restored total floor reaction force to be generated at the target total floor reaction force central point is calculated. The moment component of the actual total floor reaction force acting on the target total floor reaction force central point is the moment of the resultant total floor reaction force, target total floor reaction force, and the total floor reaction force compensated for the target reaction force balance control. The target foot position / posture is corrected so that the foot 22R (L) rotates or moves up and down so as to match the component. The corrected target foot position / posture is referred to as the final corrected target foot position / posture.
[0094]
Therefore, if the actual body position / posture detected by the inclination sensor 60 and the rate of change thereof match the corrected target body position / posture and the rate of change thereof, it acts on the target total floor reaction force central point position. The desired foot position / posture is corrected so that the moment component of the actual total floor reaction force that matches the moment component of the resultant total floor reaction force and the resultant total reaction force of the compensating total floor reaction force for object reaction force balance control.
[0095]
The leg main control device further controls the leg actuator so that the actual joint displacement follows the target leg joint displacement determined from the corrected target body position / posture and the corrected target foot position / posture.
[0096]
The leg main control system includes the leg main control device, the tilt sensor 60, the six-axis force sensor 56 provided on the foot 22R (L), the actuator driving device 88 and the actuator (for the joints 10R (L) to 20R (L). Electric motor and encoder).
[0097]
The target foot position / posture corrected by the leg main control device is sent to the object reaction force balance control device as the final corrected target foot position / posture. However, if the change in the center of gravity of the robot due to the corrected target foot position / posture can be ignored in the target reaction force balance control device, the final corrected target foot position / posture is controlled by the target reaction force balance control. There is no need to send it to the device.
[0098]
In FIG. 3, the target values input to the arm main controller are the corrected target body position / posture, the target hand position / posture, and the target object reaction force.
[0099]
In short, the functions of the main arm control device are to operate the arm actuator (electric motor such as joint 30R (L), encoder, etc.) and follow the target posture and the target object reaction force. The object reaction force control is performed simultaneously. Since it is impossible to completely satisfy the target posture and the target object reaction force at the same time, an appropriate method, for example, a conventionally known manipulator compliance control, so-called virtual compliance control is used (Mechanical Engineering Handbook, Engineering, page C4-100).
[0100]
A specific control system configuration and algorithm will be described below. The arm main control system includes the arm main control device, the six-axis force sensor 58 provided in the hand 44 (L), the actuator driving device 88, and the arm actuator (joint). 30R (L) to 42R (L) electric motor and encoder).
[0101]
The arm main control device corrects the target hand position / posture according to the difference between the actual object reaction force detected by the six-axis force sensor 58 and the target object reaction force. The corrected target hand position / posture is referred to as a final corrected target hand position / posture. The arm main control device controls the arm actuator so that the actual joint displacement follows the target arm joint displacement determined from the corrected target body position / posture and the final corrected target hand position / posture.
[0102]
Here, details of the object reaction force balance control device will be described.
[0103]
FIG. 8 is a functional block diagram showing the first half of the control configuration diagram of the object reaction force balance control device, and FIG. 9 is the second half portion of the control configuration diagram of the object reaction force balance control device.
[0104]
The process of the first half will be described with reference to FIG.
[0105]
First, since the actual hand 44R (L) is considered to be substantially in the final corrected target hand position / posture by the arm main controller, the actual object reaction force detected by the six-axis force sensor 58 is corrected. Converts the force and moment around the origin of the support leg coordinate system according to the target hand position / posture (the real hand position / posture is obtained from the actual joint displacement by kinematics calculation, and the real object reaction force is converted using this. You may).
[0106]
Next, the converted actual object reaction force is converted into a force and a moment around the target total floor reaction force center point to obtain an actual object reaction force moment around the target total floor reaction force center point. Finally, the target reaction force moment deviation around the target total floor reaction force center point is obtained by subtracting the target target reaction force moment around the target total floor reaction force center point.
[0107]
Next, with reference to FIG. 9, the process of the second half of the object reaction force balance control device will be described.
[0108]
First, the perturbation dynamics model used there will be described.
[0109]
The perturbation dynamics model is a model that expresses the relationship between the target total floor reaction force moment perturbation amount and the body position / posture perturbation amount when a constraint condition is given to the motion (perturbation) of the target work pattern. . Hereinafter, as an example, a model that perturbs the horizontal position of the upper body while keeping the upper body posture of the robot in agreement with the target body posture will be described.
[0110]
Here, the symbols are defined as follows.
m: Total robot mass
g: Gravity acceleration
h: Height of the center of gravity from the center point of the total floor reaction force
ΔxG: X component of the target center of gravity position perturbation
ΔyG: Y component of target center of gravity position perturbation
Δxb: X component of target body position perturbation
Δyb: Y component of target body position perturbation
ΔMx: X component of the desired total floor reaction force moment perturbation amount around the desired total floor reaction force center point
ΔMy: Y component of the desired total floor reaction force moment perturbation amount around the desired total floor reaction force center point
ΔMGx: X component of the target gravitational moment perturbation around the center of the target floor reaction force
ΔMGy: Y component of the target gravitational moment perturbation amount around the center of the target floor reaction force
ΔLx: X component of the perturbation amount of the target angular momentum around the target floor reaction force center point
ΔLy: Y component of the perturbation amount of the target angular momentum around the center point of the desired total floor reaction force
d (a) / dt: Time derivative of variable a
d (d (a) / dt) / dt: Time second derivative of variable a
[0111]
From the definition of the moment of gravity, the following equation is derived.

[0112]
If the equivalent moment of inertia around the center of gravity related to the robot's motion perturbation is sufficiently small and can be ignored, the following equation is derived.

[0113]
The following equation is derived from the Euler equation.

[0114]
From Equation 1, Equation 2, and Equation 3, the following equation can be obtained as the equation of motion of the perturbation dynamics model.

[0115]
By the way, it is considered that the target center-of-gravity position perturbation amount and the target body position perturbation amount are in a substantially proportional relationship. Therefore, if the proportionality constant is k, the target body position perturbation amount can be obtained by the following equation.

[0116]
From the above, the perturbation dynamic model calculates the target center-of-gravity position perturbation amount and the target body position perturbation

amount using Equations

4 and 5. Specifically, as in this device, in the case of digital calculation, Equation 4 is discretized and used. Incidentally, the equation 4 coincides with the equation of motion of an inverted pendulum having a height h and a mass m shown in FIG.
[0117]
In the latter half of the object reaction force balance control apparatus shown in FIG. 9, the object reaction force moment deviation around the target total floor reaction force center point is input to the final target gravity center perturbation amount calculation unit.
[0118]
The gravity center perturbation amount for canceling the object reaction force moment deviation around the target floor reaction force center point over a long period of time and obtaining a balance is referred to as a final target gravity center position perturbation amount. The final target center-of-gravity position perturbation amount calculation unit calculates the final target target center-of-gravity position perturbation amount from the deviation.
[0119]
here,
ΔMox: X component of object reaction force moment deviation
ΔMoy: Y component of the object reaction force moment deviation
ΔMGox: X component of the moment of gravity generated by the perturbation amount of the final target center of gravity position
ΔMGoy: Y component of the moment of gravity generated by the amount of perturbation of the final target center of gravity position
ΔxGe: X component of the final target center of gravity position perturbation
ΔyGe: Y component of the final target center of gravity position perturbation
And
[0120]
In order to cancel the object reaction force moment deviation by the gravitational moment generated by the final target gravity center position perturbation amount, it is necessary to satisfy the following equation.

[0121]
The gravitational moment generated by the final target center-of-gravity position perturbation amount is expressed by the following equation.

[0122]
From the equations 6 and 7, the following equation is obtained.

[0123]
Therefore, the final target center-of-gravity position perturbation amount may be calculated by Equation 8.
[0124]
The model control law calculator of the object reaction force balance control device will be described. The difference between the final target center-of-gravity position perturbation amount and the target center-of-gravity position perturbation amount output from the perturbation dynamics model is called a center-of-gravity displacement deviation. The model control law computing unit performs control for converging the gravity center displacement deviation to zero. The output is the compensated total floor reaction force moment for object reaction force balance control.
[0125]
Specifically, the compensation total reaction force moment for object reaction force balance control may be determined by the PD control law as shown in the following equation.

Here, Kp is a proportional gain, and Kd is a differential gain.
[0126]
The addition point immediately after the output of the model control law calculator will be described. By the addition point immediately after the output of the model control law calculator, the perturbation dynamics model includes the compensation total reaction force moment for the object reaction force balance control, The sum of the object reaction force moment deviations around the target total floor reaction force center point is input as the target total floor reaction force moment perturbation amount (model input amount) for the model, and the target body position / A posture perturbation amount is calculated. This is added to the target body position / posture to create a corrected target body position / posture.
[0127]
By the way, the perturbation dynamics model satisfies the dynamic equilibrium condition, so the target inertial moment moment perturbation amount and target gravity moment perturbation amount generated by the target body position / posture perturbation amount, which is the model output, and the sum of the model input. Is 0.
[0128]
Therefore, the following equation holds.

[0129]
On the other hand, a command is sent to the leg main controller so as to additionally generate a compensation total floor reaction force moment for target reaction force balance control around the target total floor reaction force center point. That is, the control is performed so that the compensation total floor reaction force moment for the object reaction force balance control is added to the actual total floor reaction force moment as the actual total floor reaction force moment perturbation amount. The actual total floor reaction force moment generated as a result is referred to as a corrected actual total floor reaction force moment.
[0130]
Therefore, the following equation holds.

[0131]
From Equation 11 and Equation 12, the following equation is obtained.

[0132]
By the way, the target work pattern satisfies the following equation because it satisfies the dynamic equilibrium condition.

[0133]
Each corrected moment is the original moment plus the moment perturbation, the actual object reaction force moment is the sum of the target object reaction force moment and the object reaction force moment deviation, and Equations 13 and From 14, the following equation is obtained as an identity:

[0134]
Equation 15 shows that no matter how the actual object reaction force moment deviates from the target object reaction force moment, the target inertial force moment, the target gravity moment and the actual total floor reaction force moment are corrected by the object reaction force balance control. It means that the condition 1 is satisfied.
[0135]
In other words, using Equation 13, even if an object reaction force moment deviation occurs, a target inertial force moment perturbation amount, a target gravity moment perturbation amount, and an actual total floor reaction force are controlled by the object reaction force balance control. It can be said that the moment perturbation amount is generated and the influence of the object reaction force moment deviation is canceled so as to satisfy the condition 1.
[0136]
The behavior of the object reaction force balance control with respect to the state of the cart pushing operation of FIG. 4 will be described with reference to FIG. 7 again.
[0137]
When pushing the carriage, if the absolute value of the actual rolling friction force of the carriage (that is, the target object) suddenly becomes smaller than the expected value, The force-moment deviation also changes stepwise as shown in the figure.
[0138]
On the other hand, the final target target gravity center position perturbation amount calculation unit calculates the final target target gravity center position perturbation amount. The compensation total floor reaction force moment for the object reaction force balance control is calculated by the model control law calculator according to the difference between the final target center of gravity position perturbation amount and the target center of gravity position perturbation amount.
[0139]
As shown in FIG. 7, the compensation total floor reaction force moment for the object reaction force balance control gradually approaches 0 as the target center-of-gravity position perturbation amount becomes asymptotic to the final target center-of-gravity position perturbation amount. The sum of the compensated total floor reaction force moment and the object reaction force moment deviation for the object reaction force balance control is input to the perturbation dynamics model, and the target center of gravity position perturbation amount and the target body position / posture perturbation amount are expressed as perturbation power. Output from the academic model. In this example, since the constraint is that the posture is not changed, the target body position / posture perturbation amount is zero.
[0140]
By the way, since the perturbation dynamics model satisfies the dynamic equilibrium condition, the perturbation dynamics model is the sum of the target inertial moment perturbation and target gravity moment perturbation generated by the target body position / posture perturbation. The total sum including the input moment is zero.
[0141]
That is, the sum of the target inertia force moment perturbation amount, the target gravity moment perturbation amount, the object reaction force balance control compensation total floor reaction force moment and the object reaction force moment deviation is zero. This relationship is always established as shown in FIG. The target gravity center position perturbation amount is made asymptotic to the final target gravity center position perturbation amount by the model control law calculator. The target center-of-gravity position perturbation amount changes in proportion to or approximately in proportion to the target gravity moment perturbation amount.
[0142]
The above is the behavior of the object reaction force balance control device. Note that all the above control calculations are executed for each control cycle. Therefore, the dynamic balance is always maintained whenever the actual object reaction force moment changes. In other words, when the object reaction force moment deviates from the target value, the robot 1 is initially posture-controlled so as to step on the toe (the tip of the foot 22R (L)) by manipulating the whole floor reaction force moment. At the same time, the body is switched over time so as to rely on the gravitational moment.
[0143]
FIG. 12 shows a second embodiment of the present invention, in which a limiter 200 is provided, and a limit value of an upper limit and a lower limit is set on the final target gravity center position perturbation amount so as to apply a limit.
[0144]
In the above-described embodiment, actually, if the final target center-of-gravity position perturbation amount is too large, the robot may not be able to take the posture. Therefore, in order to prevent this, a limit value (range) of an upper limit and a lower limit is set to the final reached target center-of-gravity position perturbation amount obtained by Expression 7, and the limit is applied. The limit value (limit value) may be a fixed value or a variable value.
[0145]
Furthermore, a second limiter 300 is provided, and an upper limit / lower limit value (range) is set for the compensation total floor reaction force moment for the object reaction force balance control calculated by the model control law computing unit so as to apply the limit. I made it.
[0146]
That is, the compensated total floor reaction force moment for the object reaction force balance control is also generated in the foot 22R (L) of the actual robot, but there is a limit to the total floor reaction force moment that the actual robot foot can generate, If the limit is exceeded, the grounding property of the foot is impaired, or a part of the foot floats off the floor. In order to prevent this, a limit value of upper and lower limits is set to the compensation total floor reaction force moment for the object reaction force balance control calculated by the model control law calculator using Equation 10, and the limit is applied. As in the case of the limiter 200, the limit value may be fixed or variable.
[0147]
FIG. 13 shows a third embodiment of the present invention, in which a limiter 400 is provided to set an upper limit and a lower limit on the input of the final target center of gravity position perturbation amount calculation unit, and to limit the second limiter. 500 is provided, and the compensation total floor reaction force moment for the object reaction force balance control is corrected with an input value exceeding the first limiter 400.
[0148]
In the second embodiment shown in FIG. 12, in order to prevent the final target target gravity center position perturbation amount from becoming excessive, the final target target gravity center position perturbation amount obtained by Expression 7 The limit value (range) is set and the limit is applied, but the value exceeding the limit value (limit value) is added to the output of the model control law calculator at the addition point 600 (FIG. 12) and input to the model There was something to be done. For this reason, if the center of gravity position is perturbed against the object reaction force moment deviation, the object reaction force moment deviation becomes excessive, and if the limiter is activated, the center of gravity position is swung in the opposite direction. Inconvenience may occur.
[0149]
The third embodiment solves the above-mentioned inconvenience, and an input value exceeding the limit value (limit value) of the first limiter 400 is sent to the addition / subtraction point 700 through the second limiter 500, where the model control law The compensation floor reaction force moment for object reaction force balance control was corrected so as to give a force in the opposite direction to the force to subtract from the output of the computing unit, in other words, to reverse the polarity and to invert. . As a result, the posture of the robot is controlled so that the input value (moment deviation) exceeding the limit value (limit value) is supported by the foot 22R (L).
[0150]
Unlike the second embodiment, a value exceeding the limit value is not added to the output of the model control law calculator at the addition point 710 (FIG. 13) and input to the model. For this reason, it is possible to eliminate the inconvenience that the object reaction force moment deviation as described above becomes excessive, the limiter operates, and the position of the center of gravity swings in the reverse direction. The remaining configuration is not different from the previous embodiment. Similarly to the previous embodiment, the limit value (limit value) of the first limiter 400 may be fixed or variable. Further, a limiter similar to the limiter 400 may be added after the model control law calculator.
[0151]
FIG. 14 shows a fourth embodiment of the present invention. In order to improve the accuracy of the perturbation dynamic model in the object reaction force balance control apparatus, an inverted pendulum model to which an inertial force moment I is applied is used. .
[0152]
Furthermore, if a perturbation dynamics model is used, if the height of the center of gravity does not change much, h can be fixed, but if the height of the center of gravity changes depending on the work, the final correction target body position / posture, the final correction target The height of the center of gravity may be obtained from the robot posture obtained from the foot position / posture and the final corrected target hand position / posture, and h may be changed accordingly.
[0153]
In order to further improve the accuracy of the perturbation dynamics model, it is equipped with a multi-link geometric model of a robot with leg-arm links, and the final corrected target body position / posture, final corrected target foot position / posture, and final correction The center of gravity calculated from the target hand position / posture, the body position / posture obtained by subtracting the perturbation amount of the body position from the final corrected target body position / posture, the final corrected target foot position / posture, and the final corrected target hand position -By calculating the difference between the center of gravity position obtained from the posture, the relationship between the perturbation amount of the center of gravity position and the perturbation amount of the body position with high accuracy is obtained, and the body position position is calculated from the amount of perturbation of the center of gravity position using it. The amount of perturbation may be obtained.
[0154]
This model is a multi-link dynamic model of a robot with leg-arm links. It gives a constraint condition in the movement pattern, inputs the target floor reaction force perturbation and inputs the target body position / posture perturbation amount and the center of gravity. A model that outputs a position / posture perturbation amount may be used.
[0155]
By the way, if a perturbation dynamics model that takes into account the influence of the inertial force perturbation amount and / or the gravity perturbation amount when the arm is perturbed from the target work pattern is used, the load on the control device increases.
[0156]
This is because the inertial force perturbation amount and / or gravity perturbation amount of the arm are affected by the target body position / posture perturbation amount, and conversely, the target body position / posture perturbation amount depends on the inertial force perturbation amount and / or gravity perturbation of the arm. This is because it is influenced by the amount, and calculating the model behavior considering this interaction at the same time requires a very complicated calculation.
[0157]
As means for solving the problem, the following method may be used.
[0158]
In other words, the perturbation dynamics model ignores the effects of the arm's inertial force perturbation and / or gravity perturbation when the arm is perturbed from the target work pattern, and assumes that the arm moves only according to the target motion pattern. . With this assumption, the model is approximated in the same form as the inverted pendulum model given as an example in the detailed description of the perturbation dynamics model. Therefore, the calculation of the perturbation dynamics model is very simple.
[0159]
Inertial force generated when the arm is perturbed from the target posture to the final target posture from the target hand position / posture, target body position, final corrected target hand position / posture, and final corrected target body position Calculate perturbation and / or gravity perturbation. This is obtained by performing dynamic calculation of a multi-link manipulator which is a conventional method. This is expressed in the final corrected target hand position / posture coordinate system.
[0160]
The calculated inertial force perturbation amount and / or gravity perturbation amount is added to the actual object reaction force detected by the sensor, and is output as an actual object reaction force to the object reaction force balance controller. By the above means, the influence of the inertial force perturbation amount and / or the gravity perturbation amount of the arm is considered as the work object reaction force instead of ignoring it in the perturbation dynamics model. Since the calculation of the inertial force perturbation amount and / or the gravity perturbation amount of the arm and the calculation of the perturbation dynamics model are performed independently, complicated interference calculation is not required, and the calculation amount is small.
[0161]
As described above, in the first to fourth embodiments, a leg type comprising at least a base body (upper body 28) and a plurality of links (leg link 2 and arm link 3) connected to the base body. In the attitude control device of a mobile robot (biped robot 1), a movement pattern including at least the target locus of the base of the robot, a target locus of a floor reaction force acting on the robot, and a floor reaction acting on the robot. A target gait setting means (target work pattern generator) for setting a target gait of the robot including at least a target locus of an external force other than a force, an external force detection means (six axes) for detecting an external force other than the floor reaction force Force sensor 58), an external force deviation that calculates a deviation between the detected external force and an external force other than the floor reaction force set in the target locus (an object reaction force moment deviation around the center point of the total floor reaction force). Arithmetic means (object reaction force balance control device. More specifically, coordinate conversion and input / output of actual object reaction force in FIG. 8), perturbation of the floor reaction force and the gravity center position of the robot and / or the base body Model (perturbation dynamics model) expressing the relationship of perturbation of the position of the model, model input amount to be input to the model based on at least the calculated deviation of the external force (target total floor reaction force moment perturbation amount for the model) ) For calculating the model input amount (model control law calculator and input / output at the subsequent addition point), inputting the calculated model input amount to the model, and obtaining the position of the center of gravity and / or the base body Substrate target trajectory correction amount calculating means (object reaction force balance control device, which calculates a base target trajectory correction amount (corrected target body position / posture) that corrects the target trajectory of the base in accordance with the amount of perturbation. Specifically, the perturbation dynamics model input amount is input, the behavior of the model is calculated, the target body position / posture perturbation amount (correction amount) is calculated from the model output), and at least according to the calculated model input amount Floor reaction force target locus correction amount calculating means (model control law) for calculating a floor reaction force target locus correction amount (compensated total floor reaction force moment for object reaction force balance control) for correcting the floor reaction force target locus. A computing unit, more specifically, a part of the model control law, and joint displacement means (legs) for displacing the joint of the robot based on at least the calculated base body target locus correction amount and floor reaction force target locus correction amount. Main control device, actuator driving device 88, leg actuator, etc.).
[0162]
Further, the model input amount calculation means includes equilibrium centroid position perturbation amount calculation means (final reached target centroid position perturbation amount calculation unit) that calculates a perturbation amount of an equilibrium centroid position that is statically balanced with the external force. The model input amount is calculated so that the model converges to the calculated equilibrium center-of-gravity position.
[0163]
The model is configured to be a model (perturbation dynamics model) that approximates the robot with an inverted pendulum.
[0164]
Further, the balanced centroid position perturbation amount calculating means is configured to include

limiters

200 and 400 that limit the calculated perturbation amount of the balanced centroid position to a predetermined range.
[0165]
Further, the floor reaction force target locus correction amount calculating means is configured to include

limiters

300 and 500 for limiting the calculated floor reaction force target locus correction amount to a predetermined range.
[0166]
The target locus of the floor reaction force is configured to include at least the locus of the target center point of the floor reaction force acting on the robot.
[0167]
Further, the floor reaction force target locus correction amount calculating means is configured such that the floor reaction force target locus correction amount (total reaction force moment for object reaction force balance control compensation) is converted into the model input amount (target total force for the model). The value obtained by subtracting the deviation of the external force (target reaction force moment deviation around the target total floor reaction force center point) from the floor reaction force moment perturbation amount) and the moment acting on the floor reaction force around the target center point The floor reaction force target locus correction amount is calculated so as to be balanced in terms of science.
[0168]
Further, the external force other than the floor reaction force is a reaction force from the work object (cart 100) acting on the robot via the link.
[0169]
Further, the robot is configured as a legged mobile robot including two leg links 2 and two arm links 3 connected to the base body.
[0170]
Further, in a posture control apparatus for a legged mobile robot comprising at least a base body (upper body 28) and a plurality of links (leg link 2, arm link 3) connected to the base body, at least the target of the base body of the robot A target gait setting means (target work pattern generator) for setting a target gait of the robot, including at least a movement pattern including a position and a locus of a target center point of a floor reaction force acting on the robot; An object reaction force detecting means (6-axis force sensor 58) for detecting a reaction force from the work object acting on the robot via the robot, and the detected object reaction force is transmitted around the target floor reaction force center point. Object reaction force moment conversion means (object reaction force balance control device) for converting the moment, the target reaction force moment control means to dynamically balance the converted object reaction force moment. Robot position / posture correcting means (object reaction force balance control device) for correcting the floor reaction force moment around the point and the position and posture of the robot, and the floor reaction force moment around the corrected target center point and the robot And a joint displacing means (a leg main control device, an actuator driving device 88, a leg actuator, etc.) for displacing the joint of the robot based on the position and posture of the robot.
[0171]
Further, in a posture control apparatus for a legged mobile robot comprising at least a base body (upper body 28) and a plurality of links (leg link 2, arm link 3) connected to the base body, at least the target of the base body of the robot Target gait setting means (target work pattern generator) for setting an exercise pattern including a position, and object reaction force detection means (6 for detecting reaction force from the work object acting on the robot via the link) Axial force sensor 58), object reaction force moment converting means (object reaction force balance control) for converting the detected object reaction force as a moment around a predetermined point, more specifically, a target floor reaction force center point. Apparatus), a robot that corrects the floor reaction force moment about the predetermined point and the position and posture of the robot so as to dynamically balance the converted object reaction force moment. Position / posture correction means (object reaction force balance control device), and joint displacement means for displacing the joint of the robot based on the corrected floor reaction force moment around the predetermined point and the position / posture of the robot (A leg main control device, an actuator driving device 88, a leg actuator, etc.).
[0172]
In the first to fourth embodiments described above, an actuator for bending or twisting the body link is not provided. However, when adding the actuator, a body actuator control device is also required. However, since bending or twisting of the upper body link is equivalent to adding a joint to the base of the arm or leg, it can be considered conceptually as an actuator for the arm or leg. That is, the upper body actuator control device can be considered to be included as part of the arm or leg control device.
[0173]
In the first to fourth embodiments described above, the compliance control previously proposed in JP-A-5-305586 is used, but other means may be used. For the leg control, another means other than the compliance control, for example, a means for controlling the joint torque by using a means for controlling the electric actuator by a current command type amplifier, and as a result, means for indirectly controlling the floor reaction force. If used, the 6-axis force sensor 56 provided on the foot 22R (L) is unnecessary.
[0174]
Furthermore, in the first to fourth embodiments described above, the arm torque is controlled by using another means other than the virtual compliance control, for example, a means for controlling the electric actuator by a current command type amplifier. As a result, the object reaction force may be indirectly controlled. The control does not require the 6-axis force sensor of the hand, but it is preferable to provide the 6-axis force sensor of the hand for the object reaction force balance control device.
[0175]
Furthermore, in the first to fourth embodiments described above, an estimator that estimates the actual object reaction force from the joint torque may be provided in the arm control device instead of the six-axis force sensor of the hand. This estimator may be a conventional disturbance observer.
[0176]
Further, in the first to fourth embodiments described above, in addition to the compliance control proposed in JP-A-5-305586, the control proposed by the applicant in JP-A-5-337849 may be added. good. However, since the position and stride of the upper body are corrected by the control, when the relative positional relationship between the hand and the work object is important in the arm control, the position and stride of the body corrected by the control It is necessary to consider the impact.
[0177]
Furthermore, in the above-described first to fourth embodiments, even when the floor is not a plane, the target total floor reaction is performed using a technique that assumes a virtual plane proposed by the present applicant in Japanese Patent Laid-Open No. 5-318840. The force center point and the target ZMP may be obtained on a virtual plane.
[0178]
Furthermore, in the first to fourth embodiments described above, when the posture of the entire robot is deviated from the target and tilted, the position / posture of the hand is shifted in the absolute space. As a result, the object reaction force may deviate greatly from the target object reaction force.
[0179]
In order to solve the problem, the corrected final target hand position / posture is further corrected according to the deviation between the actual body position / posture detected by the tilt sensor and the target body position / posture. Even if the posture of the entire robot is tilted, it is more preferable that the position / posture of the hand does not shift in the absolute space.
[0180]
Furthermore, in the first to fourth embodiments described above, the block diagram can be modified in various ways, such as changing the processing order.
[0181]
Furthermore, although the PD control law is used in the first to fourth embodiments described above, other control laws (for example, PID control, state feedback control) may be used.
[0182]
Further, the present invention has been described for a bipedal legged mobile robot with arms, but it is also useful for legged mobile robots without an arm, and is not limited to bipedal robots. Can be applied.
[0183]
【The invention's effect】
Even if the legged mobile robot receives an unexpected external force, more specifically, a reaction force from the work object, it can maintain a stable posture with a dynamic balance. Furthermore, the link with an operation pattern that was not anticipated in advance, more specifically, not only the gravity and inertial force generated in the arm when moving the arm, but also the unexpected reaction from the work target , You can keep a stable posture with dynamic balance.
[0184]
Also, when the reaction force of an object suddenly changes, And By moving the center of gravity of the robot to a position where the robot is tilted, tilting and falling can be effectively suppressed. Further, even in a transition period in which the center of gravity of the robot moves, it is possible to maintain the dynamic balance by appropriately changing the position of the center of gravity and the floor reaction force.
[Brief description of the drawings]
FIG. 1 is an explanatory diagram showing the overall posture control apparatus for a legged mobile robot according to the present invention.
FIG. 2 is a block diagram showing details of a control unit of the biped robot shown in FIG.
FIG. 3 is a block diagram functionally showing the configuration and operation of a posture control device for a legged mobile robot according to the present invention.
FIG. 4 is an explanatory diagram showing an operation performed by the legged mobile robot shown in FIG. 1 using an arm.
5 is an explanatory diagram showing a support leg coordinate system in a gait generated by the target work pattern generator of the apparatus of FIG. 3. FIG.
6 is an explanatory view showing a support leg coordinate system in a gait generated by the target work pattern generator of the apparatus of FIG. 3 as in FIG. 5;
7 is a timing chart for explaining the operation of the object reaction force balance control apparatus shown in FIG. 3; FIG.
8 is a first half of a block diagram showing a detailed configuration of the object reaction force balance control device shown in FIG. 3;
9 is the latter half of the block diagram showing the detailed configuration of the object reaction force balance control device shown in FIG. 3;
10 is an explanatory diagram showing a perturbation dynamic model of the object reaction force balance control device shown in FIG. 9; FIG.
FIG. 11 is an explanatory diagram showing a state in which the model shown in FIG. 10 is approximated by an inverted pendulum.
FIG. 12 is a second half of a block diagram showing a detailed configuration of an object reaction force balance control device showing a second embodiment of the present invention, similar to FIG. 9;
FIG. 13 is a second half part of a block diagram showing a detailed configuration of an object reaction force balance control device showing a third embodiment of the present invention, similar to FIG. 9;
FIG. 14 is an explanatory view showing an inverted pendulum type perturbation dynamic model similar to FIG. 11 and showing a fourth embodiment of the present invention.
[Explanation of symbols]
1 Biped walking robot (legged mobile robot)
2 leg links
3 Arm links
10, 12, 14R, L Hip joint
16R, L Knee joint
18, 20R, L Ankle joint
22R, L Foot
28 upper body
30, 32, 34R, L shoulder joint
36R, L Elbow joint
38, 40, 42R, L wrist joint
44R, L hand
50 Control unit
56,58 6-axis force sensor
60 Tilt sensor

Claims

In a posture control device for a legged mobile robot comprising at least a base and a plurality of links connected to the base,
a. The robot includes at least a movement pattern including a target locus of the base of the robot, a target locus of a floor reaction force acting on the robot, and a target locus of an external force other than the floor reaction force acting on the robot. Target gait setting means for setting the target gait,
b. An external force detecting means for detecting an external force other than the floor reaction force,
c. An external force deviation calculating means for calculating a deviation between the detected external force and an external force other than the floor reaction force set in the target locus;
d. Model expressing at least one perturbation of the relationship between the position of the center of gravity position and groups of the floor reaction force perturbation and the robot,
e. Model input amount calculation means for calculating a model input amount to be input to the model based on at least the calculated deviation of the external force;
f. Model input amount of said operation input to the model, according to at least one perturbation amount of the obtained gravity center position and base body to fix the target locus of the substrate, calculating the base target trajectory correction amount Substrate target locus correction amount calculating means
g. A floor reaction force target locus correction amount calculating means for calculating a floor reaction force target locus correction amount, which corrects the floor reaction force target locus according to at least the calculated model input amount;
And h. Joint displacement means for displacing the joint of the robot based on at least the calculated base body target locus correction amount and floor reaction force target locus correction amount;
A posture control apparatus for a legged mobile robot, comprising:

The model input amount calculation means includes:
j. A balanced center of gravity position perturbation amount calculating means for calculating a perturbation amount of a balanced center of gravity position statically balanced with the external force;
The posture control apparatus for a legged mobile robot according to claim 1, wherein the model input amount is calculated so that the model converges to the calculated equilibrium center-of-gravity position.

The posture control apparatus for a legged mobile robot according to claim 1, wherein the model is a model that approximates the robot by an inverted pendulum.

The equilibrium centroid position perturbation amount calculating means includes:
k. A limiter for limiting the perturbation amount of the calculated equilibrium centroid position to a predetermined range;
The posture control apparatus for a legged mobile robot according to claim 2, comprising:

The floor reaction force target locus correction amount calculating means is:
l. A limiter for limiting the calculated floor reaction force target locus correction amount to a predetermined range;
The posture control apparatus for a legged mobile robot according to any one of claims 1 to 4, further comprising:

The target locus of the floor reaction force, the posture control system of a legged mobile robot according 5 of Claims 1, wherein characterized in that it comprises at least the locus of the target center point of the floor reaction force acting on the robot.

The floor reaction force target trajectory correction amount calculating means is configured such that the floor reaction force target trajectory correction amount is a value obtained by subtracting the deviation of the external force from the model input amount and a moment acting around the target center point of the floor reaction force. 7. The posture control apparatus for a legged mobile robot according to claim 6, wherein the floor reaction force target trajectory correction amount is calculated so as to be dynamically balanced with the robot.

The legged mobile robot according to any one of claims 1 to 7, wherein an external force other than the floor reaction force is a reaction force from a work object acting on the robot via the link. Attitude control device.

The legged movement according to any one of claims 1 to 7, wherein the robot is a legged mobile robot including two leg links and two arm links connected to the base body. Robot posture control device.

In a posture control device for a legged mobile robot comprising at least a base and a plurality of links connected to the base,
a. Target gait setting means for setting a target gait of the robot, including at least a movement pattern including a target position of the base of the robot and a locus of a target center point of a floor reaction force acting on the robot;
b. An object reaction force detecting means for detecting a reaction force from the work object acting on the robot via the link;
c. Object reaction force moment converting means for converting the detected object reaction force as a moment about the targets in mind point,
d. A robot position / posture correcting means for correcting the floor reaction force moment around the target center point and the position and posture of the robot so as to dynamically balance the converted object reaction force moment;
And e. Joint displacement means for displacing the joint of the robot based on the floor reaction force moment around the corrected target center point and the position / posture of the robot;
A posture control apparatus for a legged mobile robot, comprising:

In a posture control device for a legged mobile robot comprising at least a base and a plurality of links connected to the base,
a. Target gait setting means for setting a movement pattern including at least a target position of the base of the robot;
b. An object reaction force detecting means for detecting a reaction force from the work object acting on the robot via the link;
c. An object reaction force moment converting means for converting the detected object reaction force as a moment around a predetermined point;
d. A robot position / posture correction means for correcting the floor reaction force moment about the predetermined point and the position and posture of the robot so as to dynamically balance the converted object reaction force moment;
And e. Joint displacement means for displacing the joint of the robot based on the floor reaction force moment around the corrected predetermined point and the position / posture of the robot;
A posture control apparatus for a legged mobile robot, comprising: