JP3672426B2 - Posture control device for legged mobile robot - Google Patents
Posture control device for legged mobile robot Download PDFInfo
- Publication number
- JP3672426B2 JP3672426B2 JP36441297A JP36441297A JP3672426B2 JP 3672426 B2 JP3672426 B2 JP 3672426B2 JP 36441297 A JP36441297 A JP 36441297A JP 36441297 A JP36441297 A JP 36441297A JP 3672426 B2 JP3672426 B2 JP 3672426B2
- Authority
- JP
- Japan
- Prior art keywords
- reaction force
- target
- robot
- floor reaction
- moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
- 238000006243 chemical reaction Methods 0.000 claims description 402
- 230000005484 gravity Effects 0.000 claims description 95
- 238000012937 correction Methods 0.000 claims description 44
- 238000004364 calculation method Methods 0.000 claims description 23
- 238000006073 displacement reaction Methods 0.000 claims description 19
- 230000005021 gait Effects 0.000 claims description 19
- 239000000758 substrate Substances 0.000 claims description 3
- 230000036544 posture Effects 0.000 description 140
- 210000002414 leg Anatomy 0.000 description 53
- 210000002683 foot Anatomy 0.000 description 41
- 210000001503 joint Anatomy 0.000 description 25
- 238000010586 diagram Methods 0.000 description 18
- 238000000034 method Methods 0.000 description 14
- 230000008859 change Effects 0.000 description 7
- 230000009471 action Effects 0.000 description 5
- 238000001514 detection method Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 5
- 210000003857 wrist joint Anatomy 0.000 description 5
- 210000000544 articulatio talocruralis Anatomy 0.000 description 4
- 210000001624 hip Anatomy 0.000 description 4
- 210000002310 elbow joint Anatomy 0.000 description 3
- 210000004394 hip joint Anatomy 0.000 description 3
- 210000000629 knee joint Anatomy 0.000 description 3
- 230000007774 longterm Effects 0.000 description 3
- 210000000323 shoulder joint Anatomy 0.000 description 3
- 230000007704 transition Effects 0.000 description 3
- 238000005452 bending Methods 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000006641 stabilisation Effects 0.000 description 2
- 238000011105 stabilization Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 210000003423 ankle Anatomy 0.000 description 1
- 230000003466 anti-cipated effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 239000003638 chemical reducing agent Substances 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000012636 effector Substances 0.000 description 1
- 210000004247 hand Anatomy 0.000 description 1
- 230000001771 impaired effect Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 210000003127 knee Anatomy 0.000 description 1
- 230000004904 long-term response Effects 0.000 description 1
- 210000001699 lower leg Anatomy 0.000 description 1
- 238000011089 mechanical engineering Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000004905 short-term response Effects 0.000 description 1
- 210000000689 upper leg Anatomy 0.000 description 1
- 210000000707 wrist Anatomy 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Description
【0001】
【発明の属する技術分野】
この発明は、脚式移動ロボットの姿勢制御装置に関し、より詳しくは脚式移動ロボット、特に2足歩行の脚式移動ロボットにおいて予期せぬ対象物反力を受けても動的バランスをとって姿勢の安定性を保つことができるようにしたものに関し、特に腕を備える脚式移動ロボットの腕と脚の脚腕協調制御としての応用が有効である。
【0002】
尚、この明細書で『対象物反力』は作業対象を含む環境から受ける外力で、ロボットに接地面から作用する床反力を除いたものを指称する意味で使用する。
【0003】
【従来の技術】
脚式移動ロボット、特に2足歩行の脚式移動ロボットで腕を備えたものとしては、「上体運動により3軸モーメントを補償する2足歩行ロボットの開発」(日本ロボット学会誌11巻第4号、1993年5月)が知られている。このロボットは単純化された腕としての振り子を備え、これを振ることによって発生する重力と慣性力も含めて目標歩容を予め設計しておき、それに追従するように歩行制御する。
【0004】
【発明が解決しようとする課題】
しかしながら、この従来技術においては腕は対象物反力を受けないことが前提となっている。従って、そこで提案されている制御を歩行だけではなく、作業にも適用したとき、作業対象から予期せぬ反作用を受けると動バランスを崩し、姿勢が不安定になったり、最悪の場合には転倒する恐れがあった。
【0005】
また、本出願人も特開平7−205069号公報で同種の脚式移動ロボットを提案しており、そこにおいては歩行時に摩擦力が低下したとき腕を振らせて安定な姿勢を回復するようにしている。
【0006】
しかしながら、本出願人が提案した脚式移動ロボットにおいては、脚と腕が協調せずに独立して別々に制御されているため、腕を駆動すると、腕が発生する重力と慣性力、および作業対象からの反作用によってロボット全体の動バランスが崩れ、却ってロボットの姿勢が不安定になる場合があった。
【0007】
従って、この発明の目的は上記した不都合を解消することにあり、脚式移動ロボットが予期できない対象物反力を受けても、動バランスをとって安定な姿勢を継続できるようにした脚式移動ロボットの姿勢制御装置を提供することにある。
【0008】
この発明の第2の目的は、対象物反力が急変するときも、それに静的にバランスする位置にロボットの重心を移動させることにより、傾きや転倒を効果的に抑制するようにした脚式移動ロボットの姿勢制御装置を提供することにある。
【0009】
この発明の第3の目的は、上記した対象物反力を受けたとき、ロボットの重心が移動する過渡期においても、重心位置や床反力を適正に変化させて動バランスを維持し続けられるようにした脚式移動ロボットの姿勢制御装置を提供することにある。
【0010】
この発明の第4の目的は、脚式移動ロボットで腕を備えるものにおいて、予め想定していなかった動作パターンで腕を動かして作業をするときに腕に発生する重力、慣性力だけでなく、作業対象から予期せぬ反作用を受けても、動バランスをとって安定な姿勢を継続できるようにした脚式移動ロボットの姿勢制御装置を提供することにある。
【0011】
【課題を解決するための手段】
上記の目的を達成するために、請求項1項にあっては、少なくとも基体と、前記基体に連結される複数本のリンクからなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標軌跡を含む運動パターンと、前記ロボットに作用する床反力の目標軌跡と、前記ロボットに作用する床反力以外の外力の目標軌跡とを少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段、前記床反力以外の外力を検出する外力検出手段、前記検出された外力と、前記目標軌跡で設定された床反力以外の外力との偏差を演算する外力偏差演算手段、前記床反力の摂動と前記ロボットの重心位置および基体の位置の少なくともいずれかの摂動の関係を表現するモデル、少なくとも前記演算された外力の偏差に基づいて前記モデルに入力すべきモデル入力量を演算するモデル入力量演算手段、前記演算されたモデル入力量を前記モデルに入力し、得られる前記重心位置および基体の少なくともいずれかの摂動量に応じて前記基体の目標軌跡を修正する、基体目標軌跡修正量を演算する基体目標軌跡修正量演算手段、少なくとも前記演算されたモデル入力量に応じて前記床反力の目標軌跡を修正する、床反力目標軌跡修正量を演算する床反力目標軌跡修正量演算手段、および、少なくとも前記演算された基体目標軌跡修正量および床反力目標軌跡修正量に基づいて前記ロボットの関節を変位させる関節変位手段、を備える如く構成した。
【0012】
ここで『位置』は、重心位置を除き、『位置および/または姿勢』を含む意味で使用する。尚、『姿勢』は後述の如く3次元空間における向きを意味する。
【0013】
ここで、『床反力の目標軌跡』とは、より具体的には、少なくとも床反力中心点の目標軌跡を含む意味で使用する。また『前記床反力の目標軌跡を修正する』とはより具体的には、床反力中心点まわりのモーメントを修正する意味で使用する。
【0014】
ここで、『外力を検出する』とは、検出のみならず外乱オブザーバなどを使用して推定することも含む意味で使用する。
【0015】
請求項2項にあっては、前記モデル入力量演算手段は、前記外力に静的に平衡する平衡重心位置の摂動量を算出する平衡重心位置摂動量算出手段、を備え、前記算出された平衡重心位置に前記モデルが収束するように前記モデル入力量を演算する如く構成した。
【0016】
請求項3項にあっては、前記モデルが前記ロボットを倒立振子で近似するモデルである如く構成した。
【0017】
請求項4項にあっては、前記平衡重心位置摂動量算出手段は、前記算出された平衡重心位置の摂動量を所定の範囲に制限するリミッタ、を備える如く構成した。
【0018】
請求項5項にあっては、前記床反力目標軌跡修正量演算手段は、前記演算された床反力目標軌跡修正量を所定の範囲に制限するリミッタ、を備える如く構成した。
【0019】
請求項6項にあっては、前記床反力の目標軌跡は、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む如く構成した。
【0020】
請求項7項にあっては、前記床反力目標軌跡修正量演算手段は、前記床反力目標軌跡修正量が、前記モデル入力量から前記外力の偏差を減算した値と、前記床反力の目標中心点まわりに作用するモーメントに動力学的に釣り合うように、前記床反力目標軌跡修正量を演算する如く構成した。
【0021】
請求項8項にあっては、前記床反力以外の外力が、前記リンクを介して前記ロボットに作用する作業対象物からの反力である如く構成した。
【0022】
請求項9項にあっては、前記ロボットが、前記基体に連結される2本の脚リンクと2本の腕リンクからなる脚式移動ロボットである如く構成した。
【0023】
請求項10項にあっては、少なくとも基体と、前記基体に連結される複数本のリンクからなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンと、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段、前記検出された対象物反力を前記目標中心点まわりのモーメントとして変換する対象物反力モーメント変換手段、前記変換された対象物反力モーメントに動力学的に釣り合うように、前記目標中心点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段、および前記修正された目標中心点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段、を備える如く構成した。
【0024】
請求項11項にあっては、少なくとも基体と、前記基体に連結される複数本のリンクからなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンを設定する目標歩容設定手段、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段、前記検出された対象物反力を所定の点まわりのモーメントとして変換する対象物反力モーメント変換手段、前記変換された対象物反力モーメントに動力学的につりあうように、前記所定の点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段、および前記修正された所定の点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段、を備える如く構成した。
【0025】
上記で、『脚式移動ロボット』は腕以外に対象物反力を受ける脚式移動ロボットを含む。また、『腕リンク』に関しては、脚リンクであっても、それが作業対象物に作用するものであれば、腕リンクとみなすものとする。例えば、昆虫型の6脚ロボットにおいて、前の2脚を用いて物を持ち上げる場合には、その脚リンクは腕リンクとみなすこととする。
【0026】
【作用】
請求項1項においては、脚式移動ロボットが予期できない外力、より具体的には作業対象物から反力を受けても、動バランスをとって安定な姿勢を継続することができる。更に、予め想定していなかった運動パターンでリンク、より具体的には腕を動かして作業をするときに腕に発生する重力、慣性力だけでなく、作業対象から予期せぬ反作用を受けても、動バランスをとって安定な姿勢を継続することができる。
【0027】
また、対象物反力が急変するときも、それに静的にバランスをとる位置にロボットの重心を移動させることにより、傾きや転倒を効果的に抑制することができる。また、ロボットの重心が移動する過渡期においても、重心位置や床反力を適正に変化させて動バランスを維持し続けることができる。
【0028】
請求項2項ないし8項においても、請求項1項と同様の作用、効果を有する。
【0029】
請求項9項にあっては、上記した作用、効果に加えて、脚式移動ロボットで腕を備えるものにおいても、予め想定していなかった動作パターンで腕を動かして作業をするときに腕に発生する重力、慣性力だけでなく、作業対象から予期せぬ反作用を受けても、動バランスをとって安定な姿勢を継続することができる。
【0030】
請求項10項ないし11項においても、請求項1項と同様の作用、効果を有する。
【発明の実施の形態】
以下、添付図面を参照してこの発明に係る脚式移動ロボットの姿勢制御装置を説明する。尚、脚式移動ロボットとしては2足歩行ロボットを例にとる。
【0031】
図1はその脚式移動ロボットの姿勢制御装置を全体的に示す概略図である。
【0032】
図示の如く、2足歩行ロボット1は左右それぞれの脚リンク2に6個の関節を備える(理解の便宜のために各関節をそれを駆動する電動モータで示す)。
【0033】
6個の関節は上から順に、腰部の脚回旋用の関節10R,10L(右側をR、左側をLとする。以下同じ)、腰部のロール軸(Y軸まわり)の関節12R,12L、同ピッチ軸(X軸まわり)の関節14R,14L、膝部のロール軸の関節16R,16L、足部のロール方向の関節18R,18L、および同ピッチ軸の関節20R,20Lから構成される。足部には足平22R,22Lが取着される。
【0034】
上記において股関節(あるいは腰関節)は関節10R(L),12R(L),14R(L)から、足関節は関節18R(L),20R(L)から構成される。また股関節と膝関節とは大腿リンク24R,24L、膝関節と足関節とは下腿リンク26R,26Lで連結される。
【0035】
更に、腰部の上位には上体(あるいは基体。リンクで示す)28が設けられると共に、その上端には左右それぞれの7個の関節からなる腕リンク3を備える(同様に、理解の便宜のために各関節をそれを駆動する電動モータで示す)。
【0036】
7個の関節は上から順に、肩部のロール軸の関節30R,30L、同ピッチ軸の関節32R,32L、腕の回旋用の関節34R,34L、肘部のロール軸の関節36R,36L、手首回旋用の関節38R,38L、同ロール軸の関節40R,40L、および同ピッチ軸の関節42R,42Lから構成される。手首の先にはハンド(エンドエフェクタ)44R,44Lが取着される。
【0037】
上記において肩関節は関節30R(L),32R(L),34R(L)から、手首関節は関節38R(L),40R(L),42R(L)から構成される。また肩関節と肘関節とは上腕リンク46R,46L、肘関節と手首関節とは下腕リンク48R,48Lで連結される。
【0038】
尚、上体(基体)28の内部には、図2に関して後述するマイクロコンピュータからなる制御ユニット50などが格納される。
【0039】
上記の構成により、脚リンク2は左右の足についてそれぞれ6つの自由度を与えられ、歩行中にこれらの6*2=12個の関節を適宜な角度で駆動することで、足全体に所望の動きを与えることができ、任意に3次元空間を歩行させることができる(この明細書で「*」は乗算を示す)。尚、前記の如く、ロボットの進行方向(ピッチ軸)をX軸、左右方向(ロール軸)をY軸、鉛直方向(重力軸)をZ軸とする。
【0040】
また、腕リンク3は左右の腕についてそれぞれ7つの自由度を与えられ、これらの7*2=14個の関節を適宜な角度で駆動することで、後述する台車を押すなどの所望の作業を行うことができる。
【0041】
図1に示す如く、足関節の下方の足平22R(L)には公知の6軸力センサ56が取着され、ロボットに作用する外力の内、接地面からロボットに作用する床反力の3方向成分Fx,Fy,Fzとモーメントの3方向成分Mx,My,Mzとを検出する。
【0042】
更に、手首関節とハンド44R(L)の間には同種の6軸力センサ58が取着され、ロボットに作用するそれ以外の外力、特に作業対象物から受ける前記した対象物反力の3方向成分Fx,Fy,Fzとモーメントの3方向成分Mx,My,Mzとを検出する。
【0043】
また、上体28には傾斜センサ60が設置され、Z軸(鉛直軸(重力軸))に対する傾きとその角速度を検出する。また各関節の電動モータはその出力を減速・増力する減速機(図示せず)を介して前記したリンク24,26R(L)などを相対変位させると共に、その回転量を検出するロータリエンコーダが設けられて変位検出器付の脚アクチュエータあるいは腕アクチュエータとして構成される。これら6軸力センサ56などの出力は制御ユニット50に送られる(図示の便宜のためロボット1の右側についてのみ図示する)。
【0044】
図2は制御ユニット50の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ60などの出力はA/D変換器70でデジタル値に変換され、その出力はバス72を介してRAM74に送られる。また各アクチュエータにおいて電動モータに隣接して配置されるエンコーダの出力は、カウンタ76を介してRAM74内に入力される。
【0045】
制御ユニット内にはCPUからなる演算装置80が設けられており、演算装置80は後述の如く、ROM84に格納されている歩容に基づいてロボットが安定な姿勢を継続することができるように、関節角変位指令(アクチュエータ変位指令)を算出し、RAM74に送出する。
【0046】
また演算装置80はRAM74からその指令と検出された実測値とを読み出し、各関節の駆動に必要な制御値(操作量)を算出してD/A変換器86と各関節に設けられたアクチュエータ駆動装置(アンプ)88を介して各関節を駆動する脚アクチュエータと腕アクチュエータの電動モータに出力する。
【0047】
図3は、この発明に係る脚式移動ロボットの姿勢制御装置(主として前記した演算装置80に相当)の構成および動作を機能的に示すブロック図である。
【0048】
この装置は脚および腕の動作を統合的に制御する装置であり、各アクチュエータ駆動装置88に対する変位指令を出力する。図示の如く、この装置は、目標作業パターン生成器、対象物反力平衡制御装置、脚メイン制御装置、および腕メイン制御装置から構成される。
【0049】
以下に、理解の便宜のため、図4に示すロボット作業状況を例に挙げて、この装置の各構成要素の処理内容を説明する。図4では、ロボット1が台車100を押しているとき、台車から受ける実対象物反力の絶対値が目標作業パターンにおいて想定していた目標対象物反力よりも突然小さくなってしまったため、このずれによってロボット1はバランスを崩し、前に傾きかけている状況とする。この実施の形態に係る装置は、このような状況においても常に動バランスを維持するように制御するものである。
【0050】
目標作業パターン生成器は、ある想定条件下において動力学的平衡条件を満足する目標作業パターンを生成する。目標作業パターンは、複数の変数の時間変化パターンによって表現される。この変数は、運動を表現する変数と環境から受ける反力を表現する変数から構成される。
【0051】
ここで、運動を表現する変数は、これによって各瞬間における姿勢が一義的に決定できる変数の組である。具体的には、目標足平位置・姿勢、目標上体位置・姿勢、目標ハンド位置・姿勢から構成される。
【0052】
また、環境から受ける反力を表現する変数は、目標全床反力中心点(位置)(目標ZMP(位置))、目標全床反力および目標対象物反力から構成される。
【0053】
これら各変数は、支持脚座標系で表される。支持脚座標系は、支持脚足首(関節18,20R(L)の交点)から足平22R(L)への垂直投影点を原点とする座標系であり、図5および図6に示すように、支持脚が接触している床に固定された座標系であり、支持脚足平の前向きをX軸の向き、左向きをY軸の向き、鉛直方向上向きをZ軸向きとする座標系である。
【0054】
以下に、これら各変数について詳細を説明する。
【0055】
前記の如く、ロボットが環境から受ける外力の内で、各足平床反力を除いた外力を対象物反力と呼ぶとき、目標対象物反力はその目標値である。図4の例ではハンド44R(L)が対象物100から受ける反力のことである。
【0056】
目標作業パターン生成器が出力する目標対象物反力は、後述する目標全床反力中心点まわりに作用する力とモーメントによって表現される。ちなみに、姿勢安定化にとって重要なのは、このうちのモーメント成分である。
【0057】
目標全床反力と目標全床反力中心点(位置)について説明すると、作業中において各足平が床から受けるべき目標床反力の合力を、広義の目標全床反力と呼ぶ。広義の目標全床反力は、目標全床反力中心点とその点における力とモーメントで表現される。目標全床反力中心点は、目標全床反力をその点を作用点とする力とモーメントで表現したとき、X軸まわりモーメント成分とY軸まわりモーメント成分が0になる床面上の点である。
【0058】
狭義の目標全床反力は、広義の目標全床反力を、目標全床反力中心点を作用点として、力とモーメントで表現した場合の力とモーメントを意味する。目標作業パターン生成器が出力する目標全床反力は、狭義の目標全床反力である。
【0059】
以降は特に説明がない限り、目標全床反力は、狭義の目標全床反力を指す。尚、平坦な床面を歩行する場合には、目標全床反力の作用点は、通常、その床面上に設定される。
【0060】
歩行制御の分野において従来から公知であるZMPの概念も、概念を次のように拡張する。即ち、ロボットの運動によって生じる慣性力と重力と対象物反力の合力が、その点を作用点とする力とモーメントで表現されたとき、X軸まわりモーメント成分とY軸まわりモーメント成分が0になる床面上の点を、ZMPと呼ぶ。ロボットが目標の運動を行う時のZMPを目標ZMP(位置)と呼ぶ。
【0061】
目標作業パターンが動力学的平衡条件を満足すると言うことは、目標作業パターンによって生じる上記の慣性力と重力と対象物反力の合力と目標全床反力が、打ち消し合って0になることである。従って、動力学的平衡条件を満足するためには、目標全床反力中心点と目標ZMPが一致しなければならない。
【0062】
目標作業パターン生成器では、動力学的平衡条件を満足する目標作業パターンを生成する。従って、目標作業パターン生成器が生成する目標全床反力中心点(位置)は目標ZMP(位置)に一致する。
【0063】
目標足平位置・姿勢、目標上体位置・姿勢、目標ハンド位置・姿勢は、前記した支持脚座標系で表現されたそれぞれの部位の位置と姿勢を表す。具体的にはこの明細書で、上体28の位置およびその速度は、上体28の重心位置などの代表点およびその(変位)速度を意味する。更に、上体あるいは足平の姿勢は、X,Y,Z空間における『向き』を意味する。
【0064】
対象物反力平衡制御装置およびその制御はこの実施の形態の制御の中心をなすもので、対象物反力平衡制御装置は姿勢バランスをとるために動力学的平衡条件を考慮しながら制御を行う。そこで、対象物反力平衡制御装置の概要を説明する前に、動力学的平衡条件について以下に説明する。
【0065】
実際のロボットの姿勢傾きの挙動を決定する最も大きな要因は、目標全床反力中心点(即ち、目標ZMP)まわりでの実際の力のモーメントのバランスである。
【0066】
目標全床反力中心点まわりに作用する力のモーメントを以下に列挙する。
1)慣性力モーメント
2)重力モーメント
3)全床反力モーメント
4)対象物反力モーメント
【0067】
以上のモーメントは先にも説明したが、改めて以下に定義する。
【0068】
慣性力モーメントは、目標全床反力中心点まわりのロボットの角運動量の変化によって生じるモーメントである。この値はオイラー方程式によって求められ、具体的には目標全床反力中心点まわりのロボットの角運動量の1階微分値の符号を反転させたものである。
【0069】
目標作業パターンの慣性力モーメントを、目標慣性力モーメントと呼ぶ。実際のロボットが作業しているときの慣性力モーメントを実慣性力モーメントと呼ぶ。
【0070】
重力モーメントは、ロボットの重心に作用する重力が目標全床反力中心点まわりに作用するモーメントである。
【0071】
各足平に作用する床反力の合力を、全床反力と呼ぶ。全床反力モーメントは、全床反力が目標全床反力中心点まわりに作用するモーメントである。
【0072】
作業対象物から受ける反力を、対象物反力と呼ぶ。対象物反力モーメントは、作業対象物反力が目標全床反力中心点まわりに作用するモーメントである。
【0073】
さて、理想的な脚メイン制御装置によって、ロボット1が目標作業パターンの運動パターンに忠実に追従していたと仮定する。このときには実慣性力モーメントは目標慣性力モーメントに一致し、実重力モーメントは目標重力モーメントに一致する。
【0074】
一方、動力学の法則(オイラー方程式)により、必ず実慣性力モーメントと実重力モーメントと実全床反力モーメントと実対象物反力モーメントの和は、0である。
【0075】
故に、ロボット1が忠実に目標作業パターンの運動パターン通りに動くためには、目標慣性力モーメントと目標重力モーメントと実全床反力モーメントと実対象物反力モーメントの和が0でなければならない。これを条件1とする。
【0076】
ところが、実際には、実対象物反力モーメントが目標対象物反力モーメントと一致せず差が生じる。例えば、図4に関して述べたように、台車を押す作業を行っているときに台車(すなわち目標対象物)の実際のころがり摩擦力の絶対値が想定していた値よりも突然小さくなってしまった状況である。
【0077】
この図の状況では、実対象物反力が目標全床反力中心点のY軸まわりに作用するモーメントは、目標対象物反力が目標全床反力中心点のY軸まわりに作用するモーメントよりも正の向きに大きくなって条件1を満たさなくなり、ロボット1は前傾する。尚、モーメントの向きは、座標軸の正方向に向いてロボット1を時計まわりに回転させるモーメントを正とする。
【0078】
このような状況においても条件1を満足させるためには、次の2通りの手法が考えられる。
【0079】
手法1)上記偏差を打ち消すように、実全床反力モーメントを変える。具体的には、目標全床反力中心点まわりに負の床反力モーメントを発生するように脚メイン制御装置に指令し、脚メイン制御装置において、この指令を受けて、足平22R(L)のつまさきを下げ、実全床反力モーメントを負の向きに増加させる。即ち、足で踏ん張るような姿勢をとらせる。
【0080】
手法2)上記偏差を打ち消すように、目標作業パターンの運動パターンを修正することにより、目標慣性力モーメントと目標重力モーメントを修正する。具体的には、目標上体位置および/または姿勢を修正することによって、目標慣性力モーメントと目標重力モーメントを修正する。即ち、上体を前に移動させる。
【0081】
この実施の形態に係る装置では両方の手法を同時に行い、短期的には手法1を主に使うことによって速い変化に対応し、長期的には手法2を主に使うことによって実全床反力モーメントを元の目標全床反力モーメントに収束させながら、常に動バランスを維持するようにした。
【0082】
実全床反力モーメントは、目標全床反力モーメントを変えるだけで脚メイン制御装置によってすばやく変化させることができるので、手法1は短期的な対応に向いている。但し、実全床反力モーメントを大きく変化させると、足平22R(L)の接地圧分布が偏って接地感が減少し、最悪の場合には足平22R(L)の一部が浮いてしまう。従って、長期的には、なるべく元の目標全床反力モーメントに戻すべきである。
【0083】
実全床反力モーメントを元の目標全床反力モーメントに戻すためには、重心位置をずらし、目標重力モーメントによって上記偏差を打ち消すように、手法2によって目標作業パターンの運動パターンを修正すれば良い。但し、重心位置を急激にずらすと、過大な目標慣性力モーメントが逆向きに発生するので、ゆっくりと重心位置をずらす必要がある。従って、手法2は長期的な対応に向いている。
【0084】
上記を前提として対象物反力平衡制御装置について説明する。対象物反力平衡制御装置は、上記の制御機能を持った装置である。
【0085】
対象物反力平衡制御装置の入力は、目標上体位置・姿勢、目標全床反力中心点(位置)、目標対象物反力、6軸力センサ58の検出値、最終修正目標ハンド位置・姿勢、最終修正目標上体位置・姿勢、最終修正目標足平位置・姿勢である(尚、近似演算を用いる場合は、最終修正目標ハンド位置・姿勢、最終修正目標上体位置・姿勢、最終修正目標足平位置・姿勢は不要である)。
【0086】
対象物反力平衡制御装置では、上記の制御機能を実現するために、目標対象物反力を実対象物反力の検出値に置き換え、それに動力学的に平衡するように目標上体位置・姿勢と目標全床反力を修正する。これにより修正された作業パターンが想定している対象物反力(即ち、修正された目標対象物反力)と実対象物反力が一致し、ロボットの動力学的平衡条件が満足される。
【0087】
対象物反力平衡制御装置の出力は、修正目標上体位置・姿勢と対象物反力平衡制御用補償全床反力である。
【0088】
修正目標上体位置・姿勢は、対象物反力平衡制御装置によって修正された目標上体位置・姿勢である。対象物反力平衡制御用補償全床反力は、目標全床反力中心点(位置)に、修正によって加えられる全床反力である。尚、対象物反力平衡制御用補償全床反力の成分の内で、姿勢安定化のための特に重要な成分は、X軸まわりモーメント成分とY軸まわりモーメント成分である。
【0089】
対象物反力平衡制御装置の出力の挙動だけを述べると、実対象物反力と目標対象物反力の偏差、より正確には両者のモーメントの偏差が急変、即ち、図7に示すようにステップ状に変化する場合には、動力学平衡条件を満足するために、最初は対象物反力平衡制御用の補償全床反力のモーメント成分が、この差に応じてすばやく応答する。
【0090】
その後しばらくすると、修正目標上体位置・姿勢が、この偏差に静的に釣り合う位置・姿勢に整定し、対象物反力平衡制御用補償全床反力のモーメント成分は0に収束する。尚、対象物反力平衡制御装置の構成とアルゴリズム説明は後述する。
【0091】
図3において、脚メイン制御装置に入力される目標値は、修正目標上体位置・姿勢、目標足平位置・姿勢、目標全床反力中心点(位置)とその点に作用する目標全床反力と対象物反力平衡制御用補償全床反力である。
【0092】
脚メイン制御装置の機能は、簡単に言うならば、脚のアクチュエータ(関節10R(L)などの電動モータおよびエンコーダ)を操作し、目標姿勢に追従する姿勢安定化制御と目標床反力に追従する床反力制御を同時に行う装置である。尚、目標姿勢と目標床反力を同時に完全に満足させることは不可能であるので、適当な調整が行われ、長期的には両方を満足するように制御される。
【0093】
より詳しくは、傾斜センサ60によって検出された実上体位置・姿勢を修正目標上体位置・姿勢に復元させるために、目標全床反力中心点に発生させるべき復元全床反力を算出し、目標全床反力中心点に作用する実全床反力のモーメント成分が、この復元全床反力と目標全床反力と対象物反力平衡制御用補償全床反力の合力のモーメント成分に一致するように、足平22R(L)を回転あるいは上下動させるべく目標足平位置・姿勢を修正する。修正された目標足平位置・姿勢を最終修正目標足平位置・姿勢と呼ぶ。
【0094】
従って、傾斜センサ60によって検出される実上体位置・姿勢とその変化率が、修正目標上体位置・姿勢とその変化率に一致しているならば、目標全床反力中心点位置に作用する実全床反力のモーメント成分が目標全床反力と対象物反力平衡制御用補償全床反力の合力のモーメント成分に一致するように目標足平位置・姿勢を修正する。
【0095】
脚メイン制御装置は、さらに、修正目標上体位置・姿勢と修正目標足平位置・姿勢から決定される目標脚関節変位に実関節変位が追従するように脚アクチュエータを制御する。
【0096】
脚メイン制御系は脚メイン制御装置、および前記した傾斜センサ60、足平22R(L)に設けた6軸力センサ56、アクチュエータ駆動装置88およびアクチュエータ(関節10R(L)ないし20R(L)用電動モータおよびエンコーダ)から構成される。
【0097】
脚メイン制御装置で修正された目標足平位置・姿勢は、最終修正目標足平位置・姿勢として対象物反力平衡制御装置に送られる。但し、対象物反力平衡制御装置において、目標足平位置・姿勢が修正されたことによるロボットの重心位置の変化が無視できるならば、最終修正目標足平位置・姿勢を対象物反力平衡制御装置に送る必要はない。
【0098】
図3において、腕メイン制御装置に入力される目標値は、修正目標上体位置・姿勢、目標ハンド位置・姿勢および目標対象物反力である。
【0099】
腕メイン制御装置の機能は簡単に言うならば、腕のアクチュエータ(関節30R(L)などの電動モータ、エンコーダ他)を操作して、目標姿勢に追従する姿勢制御と目標対象物反力に追従する対象物反力制御を同時に行うことである。目標姿勢と目標対象物反力を同時に完全に満足させることは不可能であるので、適宜な手法、例えば、従来からマニピュレータのコンプライアンス制御、いわゆる仮想コンプライアンス制御として知られるものを用いる(機械工学便覧、エンジニアリング編、C4−100頁)。
【0100】
具体的な制御系構成とアルゴリズムを以下に説明すると、腕メイン制御系は腕メイン制御装置、および前記したハンド44(L)に設けた6軸力センサ58、アクチュエータ駆動装置88および腕アクチュエータ(関節30R(L)ないし42R(L)用電動モータおよびエンコーダ)から構成される。
【0101】
腕メイン制御装置は、6軸力センサ58によって検出される実対象物反力と目標対象物反力の差に応じて目標ハンド位置・姿勢を修正する。修正された目標ハンド位置・姿勢を、最終修正目標ハンド位置・姿勢と呼ぶ。腕メイン制御装置は、修正目標上体位置・姿勢と最終修正目標ハンド位置・姿勢から決定される目標腕関節変位に実関節変位が追従するように腕アクチュエータを制御する。
【0102】
ここで、対象物反力平衡制御装置の詳細を説明する。
【0103】
図8は対象物反力平衡制御装置の制御構成図の前半部分を、図9は対象物反力平衡制御装置の制御構成図の後半部分を示す機能ブロック図である。
【0104】
図8を参照して前半部分の処理から説明する。
【0105】
先ず、実際のハンド44R(L)は、腕メイン制御装置によって、ほぼ、最終修正目標ハンド位置・姿勢にあると考えられるので、6軸力センサ58によって検出された実対象物反力を、修正目標ハンド位置・姿勢によって支持脚座標系の原点まわりの力とモーメントに変換する(実関節変位からキネマティクス演算によって、実ハンド位置・姿勢を求めて、これを用いて実対象物反力を変換しても良い)。
【0106】
次に、変換された実対象物反力を、目標全床反力中心点まわりの力とモーメントに変換することにより、目標全床反力中心点まわりの実対象物反力モーメントを得る。最後に、これから目標全床反力中心点まわりの目標対象物反力モーメントを引くことにより、目標全床反力中心点まわりの対象物反力モーメント偏差を得る。
【0107】
次に図9を参照して対象物反力平衡制御装置の後半部分の処理を説明する。
【0108】
先ず、そこで用いる摂動動力学モデルについて説明する。
【0109】
摂動動力学モデルは、目標作業パターンの運動(摂動)にある拘束条件を与えておいた場合の、目標全床反力モーメント摂動量と上体位置・姿勢摂動量との関係を表すモデルである。以下に、例として、図10に示すようにロボットの上体姿勢を目標上体姿勢に一致させたまま、上体の水平位置を摂動するモデルを説明する。
【0110】
ここで、以下のように記号をとりきめる。
m:ロボット全質量
g:重力加速度
h:目標全床反力中心点からの重心高さ
ΔxG:目標重心位置摂動量のX成分
ΔyG:目標重心位置摂動量のY成分
Δxb:目標上体位置摂動量のX成分
Δyb:目標上体位置摂動量のY成分
ΔMx:目標全床反力中心点まわりの目標全床反力モーメント摂動量のX成分
ΔMy:目標全床反力中心点まわりの目標全床反力モーメント摂動量のY成分
ΔMGx:目標全床反力中心点まわりの目標重力モーメント摂動量のX成分
ΔMGy:目標全床反力中心点まわりの目標重力モーメント摂動量のY成分
ΔLx:目標全床反力中心点まわりの目標角運動量の摂動量のX成分
ΔLy:目標全床反力中心点まわりの目標角運動量の摂動量のY成分
d(a) / dt:変数aの時間微分
d( d(a) / dt) /dt:変数aの時間2階微分
【0111】
重力モーメントの定義から、次式が導かれる。
【0112】
ロボットの運動摂動に関する重心まわりの等価慣性モーメントが十分小さく無視できるならば、次式が導かれる。
【0113】
オイラー方程式により、次式が導かれる。
【0114】
式1、式2および式3より、摂動動力学モデルの運動方程式として、次式を得ることができる。
【0115】
ところで、目標重心位置摂動量と目標上体位置摂動量は、ほぼ比例関係にあると考えられる。従って、比例定数をkとすると、次式により目標上体位置摂動量が得られる。
【0116】
以上から、摂動動力学モデルは、式4と式5を用い、目標重心位置摂動量と目標上体位置摂動量を算出する。詳しくはこの装置の如く、デジタル演算の場合には、式4は離散化して使用する。ちなみに、式4は、図11に示す、高さh、質量mの倒立振子の運動方程式に一致する。
【0117】
図9に示す対象物反力平衡制御装置後半部において、前記した目標全床反力中心点まわりの対象物反力モーメント偏差は、最終到達目標重心摂動量算出部に入力される。
【0118】
この目標全床反力中心点まわりの対象物反力モーメント偏差を長期的に打ち消してバランスをとるための重心摂動量を最終到達目標重心位置摂動量と呼ぶ。最終到達目標重心位置摂動量算出部は、上記偏差から最終到達目標重心位置摂動量を算出する。
【0119】
ここで、
ΔMox:対象物反力モーメント偏差のX成分
ΔMoy:対象物反力モーメント偏差のY成分
ΔMGox:最終到達目標重心位置摂動量によって発生する重力モーメントのX成分
ΔMGoy:最終到達目標重心位置摂動量によって発生する重力モーメントのY成分
ΔxGe:最終到達目標重心位置摂動量のX成分
ΔyGe:最終到達目標重心位置摂動量のY成分
とする。
【0120】
対象物反力モーメント偏差を最終到達目標重心位置摂動量によって発生する重力モーメントによって打ち消すためには、次式を満足する必要がある。
【0121】
最終到達目標重心位置摂動量によって発生する重力モーメントは、次式のようになる。
【0122】
式6、式7より次式を得る。
【0123】
故に、最終到達目標重心位置摂動量は、式8によって算出すれば良い。
【0124】
対象物反力平衡制御装置のモデル制御則演算器について説明すると、最終到達目標重心位置摂動量と摂動動力学モデルが出力する目標重心位置摂動量との差を、重心変位偏差と呼ぶ。モデル制御則演算器は、この重心変位偏差を0に収束させるための制御を行う。出力は、対象物反力平衡制御用補償全床反力モーメントである。
【0125】
具体的には、次式のような、PD制御則によって、対象物反力平衡制御用補償全床反力モーメントを決定すれば良い。
ここで、Kpは比例ゲイン、Kdは微分ゲインである。
【0126】
モデル制御則演算器の出力直後の加算点について説明すると、モデル制御則演算器の出力直後の加算点によって、摂動動力学モデルには、対象物反力平衡制御用補償全床反力モーメントと、目標全床反力中心点まわりの対象物反力モーメント偏差の和が、モデルのための目標全床反力モーメント摂動量(モデル入力量)として入力され、その入力に対応する目標上体位置・姿勢摂動量が算出される。これが目標上体位置・姿勢に加算され、修正目標上体位置・姿勢が作られる。
【0127】
ところで、摂動動力学モデルは動力学的平衡条件を満たすので、モデル出力である目標上体位置・姿勢摂動量によって発生する目標慣性力モーメント摂動量および目標重力モーメント摂動量と、モデル入力との和は0である。
【0128】
故に、次式が成立する。
【0129】
一方、脚メイン制御装置には、目標全床反力中心点まわりに対象物反力平衡制御用補償全床反力モーメントを付加的に発生するように指令が送られる。即ち、実全床反力モーメントに、実全床反力モーメント摂動量として、対象物反力平衡制御用補償全床反力モーメントが加えられるように制御される。その結果発生する、実全床反力モーメントを修正実全床反力モーメントと呼ぶ。
【0130】
故に、次式が成立する。
【0131】
式11と式12から、次式が得られる。
【0132】
ところで、目標作業パターンは動力学的平衡条件を満足しているから、次式を満足する。
【0133】
各修正モーメントが元のモーメントにモーメント摂動量を加えたものであること、実対象物反力モーメントが目標対象物反力モーメントと対象物反力モーメント偏差の和であること、および式13と式14から、恒等式として次式が得られる。
【0134】
式15は、実対象物反力モーメントがいかに目標対象物反力モーメントからずれようとも、対象物反力平衡制御によって目標慣性力モーメント、目標重力モーメントおよび実全床反力モーメントが修正され、常に条件1を満たしていることを意味する。
【0135】
同一のことを式13を用いて言い換えると、対象物反力モーメント偏差が発生しても、対象物反力平衡制御によって、目標慣性力モーメント摂動量、目標重力モーメント摂動量および実全床反力モーメント摂動量が発生し、条件1を満足するように対象物反力モーメント偏差の影響を打ち消していると言える。
【0136】
図4の台車押し作業の状況に対する対象物反力平衡制御の挙動を、図7を再び参照して説明する。
【0137】
台車を押す作業を行っているとき、台車(即ち、目標対象物)の実際のころがり摩擦力の絶対値が想定していた値よりも突然ステップ状に小さくなってしまった状況では、対象物反力モーメント偏差も、図のようにステップ状に変化する。
【0138】
これに対し、最終到達目標重心位置摂動量算出部が、最終到達目標重心位置摂動量を算出する。モデル制御則演算器により最終到達目標重心位置摂動量と目標重心位置摂動量の差に応じて、対象物反力平衡制御用補償全床反力モーメントが算出される。
【0139】
図7に示すように、対象物反力平衡制御用補償全床反力モーメントは、目標重心位置摂動量が最終到達目標重心位置摂動量に漸近するにつれて0に漸近する。摂動動力学モデルには対象物反力平衡制御用補償全床反力モーメントと対象物反力モーメント偏差の和が入力され、目標重心位置摂動量と目標上体位置・姿勢摂動量が、摂動動力学モデルから出力される。尚、この例では、姿勢を変えないことが拘束条件であるので、目標上体位置・姿勢摂動量は0である。
【0140】
ところで、摂動動力学モデルが動力学的平衡条件を満足することから、目標上体位置・姿勢摂動量によって発生する目標慣性力モーメント摂動量と目標重力モーメント摂動量の和に、摂動動力学モデルに入力されたモーメントを加えた総和は0である。
【0141】
即ち、目標慣性力モーメント摂動量、目標重力モーメント摂動量、対象物反力平衡制御用補償全床反力モーメントと対象物反力モーメント偏差の和は0になる。この関係は、図7に示すように、常に成立する。目標重心位置摂動量は、モデル制御則演算器によって最終到達目標重心位置摂動量に漸近させられる。目標重心位置摂動量は、目標重力モーメント摂動量に比例あるいはほぼ比例して変化する。
【0142】
以上が、対象物反力平衡制御装置の挙動である。尚、上記制御演算は、全て、制御周期毎に実行される。従って、実対象物反力モーメントがいつ変化しても、常に、動バランスは維持される。換言すれば、対象物反力モーメントが目標値からずれたとき、ロボット1は最初は全床反力モーメントを操作してつまさき(足平22R(L)の先端)を踏ん張るように姿勢制御されると共に、経時的に上体を前方に移動させて重力モーメントに頼るように切り換えられる。
【0143】
図12はこの発明の第2の実施の形態を示し、リミッタ200を設け、最終到達目標重心位置摂動量に上限下限の制限値を設定してリミットをかけるようにしたものである。
【0144】
上記した実施の形態において、実際には、最終到達目標重心位置摂動量をあまり大きくすると、ロボットの姿勢がとれなくなる場合が生じる。従って、これを防ぐために、式7によって得られた最終到達目標重心位置摂動量に、上限下限の制限値(範囲)を設定してリミットをかけるようにした。尚、その制限値(リミット値)は固定値でも良く、あるいは可変値としても良い。
【0145】
更に、第2のリミッタ300を設け、モデル制御則演算器で演算された対象物反力平衡制御用補償全床反力モーメントにも上限下限の制限値(範囲)を設定し、リミットをかけるようにした。
【0146】
即ち、対象物反力平衡制御用補償全床反力モーメントは実ロボットの足平22R(L)にも発生させるが、実ロボットの足平が発生できる全床反力モーメントには限度があり、限度を超えると、足平の接地性が損なわれたり、足平の一部が床から浮いたりする。それを防ぐためには、モデル制御則演算器が式10を用いて演算した対象物反力平衡制御用補償全床反力モーメントに上限下限の制限値を設定し、リミットをかけるようにした。その制限値が固定でも可変でも良いことはリミッタ200の場合と同様である。
【0147】
図13はこの発明の第3の実施の形態を示し、リミッタ400を設けて最終到達目標重心位置摂動量算出部の入力に上限下限の制限値を設定してリミットをかけると共に、第2のリミッタ500を設け、第1のリミッタ400を超えた入力値で対象物反力平衡制御用補償全床反力モーメントを修正するようにした。
【0148】
図12に示した第2の実施の形態においては、最終到達目標重心位置摂動量が過大になるのを防止するために、式7によって得られた最終到達目標重心位置摂動量に、上限下限の制限値(範囲)を設定してリミットをかけるようにしたが、制限値(リミット値)を超えた値がモデル制御則演算器の出力に加算点600(図12)において加算され、モデルに入力されることがあった。このため、対象物反力モーメント偏差に抗して重心位置を摂動していたものが、対象物反力モーメント偏差が過大になり、リミッタが作動すると、重心位置を逆方向に揺動してしまうと言う不都合を生じることがあった。
【0149】
第3の実施の形態は上記した不都合を解消するもので、第1のリミッタ400の制限値(リミット値)を超えた入力値を第2のリミッタ500を通して加減算点700に送り、そこでモデル制御則演算器の出力から減算、換言すれば極性を反転させて転倒させようとする力と逆向きの力を与えるように、対象物反力平衡制御用補償全床反力モーメントを修正するようにした。これによって、ロボットは制限値(リミット値)を超えた入力値(モーメント偏差)を足平22R(L)で支持するように姿勢制御される。
【0150】
第2の実施の形態と異なり、リミット値を超えた値がモデル制御則演算器の出力に加算点710(図13)において加算されてモデルに入力されることがない。このため、前記したような対象物反力モーメント偏差が過大になってリミッタが作動し、重心位置を逆方向に揺動してしまうと言う不都合を解消することができる。尚、残余の構成は従前の実施の形態と異ならない。第1のリミッタ400の制限値(リミット値)が固定でも可変でも良いことも、従前の実施の形態と同様である。さらに、モデル制御則演算器の後に、リミッタ400と同様のリミッタを追加しても良い。
【0151】
図14はこの発明の第4の実施の形態を示し、対象物反力平衡制御装置において摂動動力学モデルの精度を高めるために、慣性力モーメントIを与えた倒立振子モデルを使用するようにした。
【0152】
更に、摂動動力学モデルについて敷衍すると、重心高さがあまり変わらないならば、hは固定で良いが、作業によって重心高さが変わる場合には、最終修正目標上体位置・姿勢、最終修正目標足平位置・姿勢および最終修正目標ハンド位置・姿勢から求められるロボット姿勢から重心高さを求め、これに応じてhを変更しても良い。
【0153】
また、摂動動力学モデルの精度をより高めるために、脚腕のリンクを持つロボットの多リンク幾何学モデルを備え、最終修正目標上体位置・姿勢、最終修正目標足平位置・姿勢および最終修正目標ハンド位置・姿勢から求められる重心位置と、最終修正目標上体位置・姿勢から上体位置の摂動量を差し引いた上体位置・姿勢、最終修正目標足平位置・姿勢および最終修正目標ハンド位置・姿勢から求められる重心位置との差を求めることにより、高精度な重心位置の摂動量と上体位置の摂動量との関係を求め、それを用いて重心位置の摂動量から上体位置の摂動量を求めても良い。
【0154】
また、脚腕のリンクを持つロボットの多リンク動力学モデルであって、運動パターンにある拘束条件を与えておいて、目標床反力の摂動を入力として目標上体位置・姿勢摂動量と重心位置・姿勢摂動量を出力させるモデルを用いても良い。
【0155】
ところで、腕を目標作業パターンから摂動させたときの腕の慣性力摂動量およびまたは重力摂動量の影響を考慮した摂動動力学モデルを用いると、制御装置の負荷が大きくなる。
【0156】
何故なら、腕の慣性力摂動量およびまたは重力摂動量は、目標上体位置・姿勢摂動量に影響され、逆に目標上体位置・姿勢摂動量は、腕の慣性力摂動量およびまたは重力摂動量に影響されるので、この相互作用を同時に考慮してモデル挙動を算出することは、非常に複雑な演算が必要となるからである。
【0157】
その問題を解決する手段として、以下の手法を用いても良い。
【0158】
即ち、摂動動力学モデルでは、腕を目標作業パターンから摂動させたときの腕の慣性力摂動量およびまたは重力摂動量の影響を無視し、腕は目標動作パターン通りにしか動かないものと仮定する。この仮定により、モデルは摂動動力学モデルの詳細説明に例として挙げた倒立振子モデルと同一形式に近似される。従って、摂動動力学モデルの演算は極めて簡単になる。
【0159】
腕メイン制御装置において、目標ハンド位置・姿勢、目標上体位置、最終修正目標ハンド位置・姿勢および最終修正目標上体位置から、腕を目標姿勢から最終目標姿勢に摂動させたために生じた慣性力摂動量およびまたは重力摂動量を算出する。これは、従来からの手法である多リンクマニピュレータの動力学演算を行うことにより得られる。これは、最終修正目標ハンド位置・姿勢座標系で表す。
【0160】
算出された慣性力摂動量およびまたは重力摂動量を、センサによって検出された実対象物反力に加え、対象物反力平衡制御装置に実対象物反力として出力する。以上の手段により、腕の慣性力摂動量およびまたは重力摂動量の影響を、摂動動力学モデルにおいて無視した代わりに、作業対象物反力として考慮したこととなる。腕の慣性力摂動量およびまたは重力摂動量の演算と摂動動力学モデルの演算が独立して行われるので、複雑な干渉演算が不要となり、演算量が小さくて済む。
【0161】
上記の如く、第1ないし第4の実施の形態にあっては、少なくとも基体(上体28)と、前記基体に連結される複数本のリンク(脚リンク2および腕リンク3)からなる脚式移動ロボット(2足歩行ロボット1)の姿勢制御装置において、前記ロボットの少なくとも前記基体の目標軌跡を含む運動パターンと、前記ロボットに作用する床反力の目標軌跡と、前記ロボットに作用する床反力以外の外力の目標軌跡とを少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段(目標作業パターン生成器)、前記床反力以外の外力を検出する外力検出手段(6軸力センサ58)、前記検出された外力と、前記目標軌跡で設定された床反力以外の外力の偏差(目標全床反力中心点まわりの対象物反力モーメント偏差)を演算する外力偏差演算手段(対象物反力平衡制御装置。より具体的には、図8の実対象物反力の座標変換およびその入出力)、前記床反力の摂動と前記ロボットの重心位置および/または基体の位置の摂動の関係を表現するモデル(摂動動力学モデル)、少なくとも前記演算された外力の偏差に基づいて前記モデルに入力すべきモデル入力量(モデルのための目標全床反力モーメント摂動量)を演算するモデル入力量演算手段(モデル制御則演算器およびその後の加算点での入出力)、前記演算されたモデル入力量を前記モデルに入力し、得られる前記重心位置および/または基体の摂動量に応じて前記基体の目標軌跡を修正する、基体目標軌跡修正量(修正目標上体位置・姿勢)を演算する基体目標軌跡修正量演算手段(対象物反力平衡制御装置。より具体的には、摂動動力学モデル入力量を入力し、モデルの挙動を演算し、モデル出力から目標上体位置姿勢摂動量(修正量)を求める部分)、少なくとも前記演算されたモデル入力量に応じて前記床反力の目標軌跡を修正する、床反力目標軌跡修正量(対象物反力平衡制御用補償全床反力モーメント)を演算する床反力目標軌跡修正量演算手段(モデル制御則演算器、より具体的にはモデル制御則の一部)、および少なくとも前記演算された基体目標軌跡修正量および床反力目標軌跡修正量に基づいて前記ロボットの関節を変位させる関節変位手段(脚メイン制御装置、アクチュエータ駆動装置88、脚アクチュエータなど)、を備える如く構成した。
【0162】
また、前記モデル入力量演算手段は、前記外力に静的に平衡する平衡重心位置の摂動量を算出する平衡重心位置摂動量算出手段(最終到達目標重心位置摂動量算出部)、を備え、前記算出された平衡重心位置に前記モデルが収束するように前記モデル入力量を演算する如く構成した。
【0163】
また、前記モデルが前記ロボットを倒立振子で近似するモデル(摂動動力学モデル)である如く構成した。
【0164】
また、前記平衡重心位置摂動量算出手段は、前記算出された平衡重心位置の摂動量を所定の範囲に制限するリミッタ200,400を備える如く構成した。
【0165】
また、前記床反力目標軌跡修正量演算手段は、前記演算された床反力目標軌跡修正量を所定の範囲に制限するリミッタ300,500を備えるように構成した。
【0166】
また、前記床反力の目標軌跡は、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む如く構成した。
【0167】
また、前記床反力目標軌跡修正量演算手段は、前記床反力目標軌跡修正量(対象物反力平衡制御補償用全床反力モーメント)が、前記モデル入力量(モデルのための目標全床反力モーメント摂動量)から前記外力の偏差(目標全床反力中心点まわりの対象物反力モーメント偏差)を減算した値と、前記床反力の目標中心点まわりに作用するモーメントに動力学的に釣り合うように、前記床反力目標軌跡修正量を演算する如く構成した。
【0168】
また、前記床反力以外の外力が、前記リンクを介して前記ロボットに作用する作業対象物(台車100)からの反力である如く構成した。
【0169】
また、前記ロボットが、前記基体に連結される2本の脚リンク2と2本の腕リンク3からなる脚式移動ロボットである如く構成した。
【0170】
また、少なくとも基体(上体28)と、前記基体に連結される複数本のリンク(脚リンク2、腕リンク3)からなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンと、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段(目標作業パターン生成器)、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段(6軸力センサ58)、前記検出された対象物反力を前記目標床反力中心点まわりのモーメントとして変換する対象物反力モーメント変換手段(対象物反力平衡制御装置)、前記変換された対象物反力モーメントに動力学的に釣り合うように、前記目標中心点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段(対象物反力平衡制御装置)、および前記修正された目標中心点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段(脚メイン制御装置、アクチュエータ駆動装置88、脚アクチュエータなど)、を備える如く構成した。
【0171】
また、少なくとも基体(上体28)と、前記基体に連結される複数本のリンク(脚リンク2、腕リンク3)からなる脚式移動ロボットの姿勢制御装置において、前記ロボットの少なくとも前記基体の目標位置を含む運動パターンを設定する目標歩容設定手段(目標作業パターン生成器)、前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段(6軸力センサ58)、前記検出された対象物反力を所定の点、より具体的には目標床反力中心点まわりのモーメントとして変換する対象物反力モーメント変換手段(対象物反力平衡制御装置)、前記変換された対象物反力モーメントに動力学的につりあうように、前記所定の点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段(対象物反力平衡制御装置)、および前記修正された所定の点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段(脚メイン制御装置、アクチュエータ駆動装置88、脚アクチュエータなど)、を備える如く構成した。
【0172】
尚、上記した第1ないし第4の実施の形態においては、上体リンクの曲げやひねりのためのアクチュエータを設けなかったが、それを追加するとき、上体アクチュエータ制御装置も必要となる。但し、上体リンクの曲げやひねりは腕または脚の付け根側に関節を追加したことと等価であるので、概念上、腕または脚のアクチュエータとみなすことができる。即ち、上体アクチュエータ制御装置は、腕または脚の制御装置の一部として含まれると考えることができる。
【0173】
上記した第1ないし第4の実施の形態では、先に特開平5−305586号公報で提案したコンプライアンス制御を用いているが、それ以外の手段を用いても良い。脚制御に、そのコンプライアンス制御以外の別の手段、たとえば、電動アクチュエータを電流指令型のアンプによって制御する手段を用いて関節トルクを制御し、その結果、間接的に床反力を制御する手段を用いれば、足平22R(L)に設けた6軸力センサ56は不要である。
【0174】
更に、上記した第1ないし第4の実施の形態において、腕の制御に、仮想コンプライアンス制御以外の別の手段、たとえば、電動アクチュエータを電流指令型のアンプによって制御する手段を用いて関節トルクを制御し、その結果、間接的に対象物反力を制御しても良い。その制御ではハンドの6軸力センサは不要であるが、対象物反力平衡制御装置のために、ハンドの6軸力センサは設けるのが良い。
【0175】
更に、上記した第1ないし第4の実施の形態において、ハンドの6軸力センサの代わりに、関節トルクから実対象物反力を推定する推定器を腕制御装置に備えても良い。この推定器は、従来技術である外乱オブザーバーを用いれば良い。
【0176】
更には、上記した第1ないし第4の実施の形態において、特開平5−305586号で提案したコンプライアンス制御に加えて、本出願人が特開平5−337849号公報で提案した制御を加えても良い。但し、その制御によって上体の位置や歩幅が修正されるので、腕制御においてハンドと作業対象物との相対位置関係が重要な場合には、その制御によって修正される上体の位置や歩幅の影響を考慮する必要がある。
【0177】
更には、上記した第1ないし第4の実施の形態において、床が平面でない場合でも、本出願人が特開平5−318840号公報で提案した仮想平面を想定する技術を用いて目標全床反力中心点や目標ZMPを仮想平面上に求めても良い。
【0178】
更には、上記した第1ないし第4の実施の形態において、ロボット全体の姿勢が目標からずれて傾くと、ハンドの位置・姿勢が絶対空間においてずれる。この結果、対象物反力が目標対象物反力から大きくずれる場合がある。
【0179】
その問題点を解決するために、傾斜センサによって検出される実上体位置・姿勢と目標上体位置・姿勢のずれに応じて上記の修正された最終目標ハンド位置・姿勢をさらに補正することにより、ロボット全体の姿勢が傾いても、ハンドの位置・姿勢が絶対空間においてずれないようにするのが、より好ましい。
【0180】
更には、上記した第1ないし第4の実施の形態において、ブロック図は演算処理順序を変えるなど、種々の変形が可能である。
【0181】
更には、上記した第1ないし第4の実施の形態においてはPD制御則を用いたが、それ以外の制御則(たとえば、PID制御、状態フィードバック制御)などを用いても良い。
【0182】
また、この発明を腕を備えた2足歩行の脚式移動ロボットについて説明したが、腕を備えない脚式移動ロボットにも有益であり、更に2足歩行ロボットに限らず、多脚ロボットにも応用することができる。
【0183】
【発明の効果】
脚式移動ロボットが予期できない外力、より具体的には作業対象物から反力を受けても、動バランスをとって安定な姿勢を継続することができる。更に、予め想定していなかった動作パターンでリンク、より具体的には腕を動かして作業をするときに腕に発生する重力、慣性力だけでなく、作業対象から予期せぬ反作用を受けても、動バランスをとって安定な姿勢を継続することができる。
【0184】
また、対象物反力が急変するときも、それに静的にバランスをとる位置にロボットの重心を移動させることにより、傾きや転倒を効果的に抑制することができる。また、ロボットの重心が移動する過渡期においても、重心位置や床反力を適正に変化させて動バランスを維持し続けることができる。
【図面の簡単な説明】
【図1】この発明に係る脚式移動ロボットの姿勢制御装置を全体的に示す説明図である。
【図2】図1に示す2足歩行ロボットの制御ユニットの詳細を示すブロック図である。
【図3】この発明に係る脚式移動ロボットの姿勢制御装置の構成および動作を機能的に示すブロック図である。
【図4】図1に示す脚式移動ロボットが腕を使用して行う作業を示す説明図である。
【図5】図3装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。
【図6】図5と同様に、図3装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。
【図7】図3に示す対象物反力平衡制御装置の動作を説明するタイミング・チャートである。
【図8】図3に示す対象物反力平衡制御装置の詳細な構成を示すブロック図の前半部である。
【図9】図3に示す対象物反力平衡制御装置の詳細な構成を示すブロック図の後半部である。
【図10】図9に示す対象物反力平衡制御装置の摂動動力学モデルを示す説明図である。
【図11】図10に示すモデルを倒立振子で近似した状態を示す説明図である。
【図12】図9に類似する、この発明の第2の実施の形態を示す対象物反力平衡制御装置の詳細な構成を示すブロック図の後半部である。
【図13】図9に類似する、この発明の第3の実施の形態を示す対象物反力平衡制御装置の詳細な構成を示すブロック図の後半部である。
【図14】図11に類似する、この発明の第4の実施の形態を示す倒立振子型摂動動力学モデルを示す説明図である。
【符号の説明】
1 2足歩行ロボット(脚式移動ロボット)
2 脚リンク
3 腕リンク
10,12,14R,L 腰関節
16R,L 膝関節
18,20R,L 足関節
22R,L 足平
28 上体
30,32,34R,L 肩関節
36R,L 肘関節
38,40,42R,L 手首関節
44R,L ハンド
50 制御ユニット
56,58 6軸力センサ
60 傾斜センサ[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
[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
[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]
[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
[0033]
The six joints are, in order from the top,
[0034]
In the above, the hip joint (or waist joint) is composed of
[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
[0036]
The seven joints are, in order from the top, shoulder
[0037]
In the above, the shoulder joint is composed of
[0038]
Note that a
[0039]
With the above configuration, the
[0040]
Also, the
[0041]
As shown in FIG. 1, a known 6-
[0042]
Furthermore, a 6-
[0043]
In addition, an
[0044]
FIG. 2 is a block diagram showing the details of the
[0045]
In the control unit, an
[0046]
The
[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
[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
[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
[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
[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
[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
[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
[0078]
In order to satisfy the
[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
[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
[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,
[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
[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-
[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
[0094]
Therefore, if the actual body position / posture detected by the
[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
[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-
[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-
[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
[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
[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
[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
[0123]
Therefore, the final target center-of-gravity position perturbation amount may be calculated by
[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
[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
[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
[0143]
FIG. 12 shows a second embodiment of the present invention, in which a
[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
[0146]
That is, the compensated total floor reaction force moment for the object reaction force balance control is also generated in the
[0147]
FIG. 13 shows a third embodiment of the present invention, in which a
[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
[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
[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 (
[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
[0165]
Further, the floor reaction force target locus correction amount calculating means is configured to include
[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
[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 (
[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 (
[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-
[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 (11)
a.前記ロボットの少なくとも前記基体の目標軌跡を含む運動パターンと、前記ロボットに作用する床反力の目標軌跡と、前記ロボットに作用する床反力以外の外力の目標軌跡とを少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段、
b.前記床反力以外の外力を検出する外力検出手段、
c.前記検出された外力と、前記目標軌跡で設定された床反力以外の外力との偏差を演算する外力偏差演算手段、
d.前記床反力の摂動と前記ロボットの重心位置および基体の位置の少なくともいずれかの摂動の関係を表現するモデル、
e.少なくとも前記演算された外力の偏差に基づいて前記モデルに入力すべきモデル入力量を演算するモデル入力量演算手段、
f.前記演算されたモデル入力量を前記モデルに入力し、得られる前記重心位置および基体の少なくともいずれかの摂動量に応じて前記基体の目標軌跡を修正する、基体目標軌跡修正量を演算する基体目標軌跡修正量演算手段、
g.少なくとも前記演算されたモデル入力量に応じて前記床反力の目標軌跡を修正する、床反力目標軌跡修正量を演算する床反力目標軌跡修正量演算手段、
および
h.少なくとも前記演算された基体目標軌跡修正量および床反力目標軌跡修正量に基づいて前記ロボットの関節を変位させる関節変位手段、
を備えたことを特徴とする脚式移動ロボットの姿勢制御装置。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:
j.前記外力に静的に平衡する平衡重心位置の摂動量を算出する平衡重心位置摂動量算出手段、
を備え、前記算出された平衡重心位置に前記モデルが収束するように前記モデル入力量を演算することを特徴とする請求項1項記載の脚式移動ロボットの姿勢制御装置。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.
k.前記算出された平衡重心位置の摂動量を所定の範囲に制限するリミッタ、
を備えることを特徴とする請求項2項または3項記載の脚式移動ロボットの姿勢制御装置。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:
l.前記演算された床反力目標軌跡修正量を所定の範囲に制限するリミッタ、
を備えることを特徴とする請求項1項から4項のいずれかに記載の脚式移動ロボットの姿勢制御装置。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:
a.前記ロボットの少なくとも前記基体の目標位置を含む運動パターンと、前記ロボットに作用する床反力の目標中心点の軌跡を少なくとも含む、前記ロボットの目標歩容を設定する目標歩容設定手段、
b.前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段、
c.前記検出された対象物反力を前記目標中心点まわりのモーメントとして変換する対象物反力モーメント変換手段、
d.前記変換された対象物反力モーメントに動力学的に釣り合うように、前記目標中心点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段、
および
e.前記修正された目標中心点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段、
を備えたことを特徴とする脚式移動ロボットの姿勢制御装置。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:
a.前記ロボットの少なくとも前記基体の目標位置を含む運動パターンを設定する目標歩容設定手段、
b.前記リンクを介して前記ロボットに作用する、作業対象物からの反力を検出する対象物反力検出手段、
c.前記検出された対象物反力を所定の点まわりのモーメントとして変換する対象物反力モーメント変換手段、
d.前記変換された対象物反力モーメントに動力学的につりあうように、前記所定の点まわりの床反力モーメントと前記ロボットの位置および姿勢を修正するロボット位置・姿勢修正手段、
および
e.前記修正された所定の点まわりの床反力モーメントと前記ロボットの位置・姿勢に基づいて前記ロボットの関節を変位させる関節変位手段、
を備えたことを特徴とする脚式移動ロボットの姿勢制御装置。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:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP36441297A JP3672426B2 (en) | 1996-12-19 | 1997-12-19 | Posture control device for legged mobile robot |
Applications Claiming Priority (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP8-354561 | 1996-12-19 | ||
JP35456196 | 1996-12-19 | ||
JP36441297A JP3672426B2 (en) | 1996-12-19 | 1997-12-19 | Posture control device for legged mobile robot |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH10230485A JPH10230485A (en) | 1998-09-02 |
JP3672426B2 true JP3672426B2 (en) | 2005-07-20 |
Family
ID=26580085
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP36441297A Expired - Lifetime JP3672426B2 (en) | 1996-12-19 | 1997-12-19 | Posture control device for legged mobile robot |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP3672426B2 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103895016A (en) * | 2012-12-27 | 2014-07-02 | 现代自动车株式会社 | Method for controlling gait of robot |
Families Citing this family (28)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4481132B2 (en) * | 1999-11-25 | 2010-06-16 | ソニー株式会社 | Robot apparatus and control method thereof |
DE60143101D1 (en) * | 2000-11-20 | 2010-10-28 | Sony Corp | Motion control and motion control method for a legged robot with legs and robotic device |
JP3790816B2 (en) | 2002-02-12 | 2006-06-28 | 国立大学法人 東京大学 | Motion generation method for humanoid link system |
EP1502711B1 (en) | 2002-04-26 | 2011-02-23 | Honda Giken Kogyo Kabushiki Kaisha | Control device of legged mobile robot |
JP3833567B2 (en) | 2002-05-01 | 2006-10-11 | 本田技研工業株式会社 | Mobile robot attitude control device |
JP4168943B2 (en) * | 2004-01-28 | 2008-10-22 | トヨタ自動車株式会社 | Legged robot and walking control method for legged robot |
JP4828424B2 (en) * | 2004-08-02 | 2011-11-30 | 本田技研工業株式会社 | Control method for legged mobile robot |
JP4485279B2 (en) * | 2004-08-02 | 2010-06-16 | 本田技研工業株式会社 | Gait generation device and control device for legged mobile robot |
JP4501601B2 (en) * | 2004-09-03 | 2010-07-14 | トヨタ自動車株式会社 | Legged robot, its control method, gait data creation device and creation method |
DE602005021689D1 (en) | 2004-10-15 | 2010-07-15 | Honda Motor Co Ltd | GANGERZEUGER FOR MOBILE ROBOT WITH LEGS |
JP4531520B2 (en) * | 2004-10-15 | 2010-08-25 | 本田技研工業株式会社 | Control device for legged mobile robot |
JP4384021B2 (en) | 2004-12-14 | 2009-12-16 | 本田技研工業株式会社 | Legged robot controller |
JP4440761B2 (en) | 2004-12-24 | 2010-03-24 | 本田技研工業株式会社 | Control device for legged mobile robot |
US7835822B2 (en) * | 2005-03-30 | 2010-11-16 | Honda Motor Co., Ltd. | Systems and methods for controlling a legged robot using a two-phase disturbance response strategy |
JP4641252B2 (en) * | 2005-12-12 | 2011-03-02 | 本田技研工業株式会社 | Gait generator for legged mobile robot |
JP4914703B2 (en) * | 2005-12-12 | 2012-04-11 | 本田技研工業株式会社 | Control device for legged mobile robot |
JP5109573B2 (en) | 2007-10-19 | 2012-12-26 | ソニー株式会社 | Control system, control method, and robot apparatus |
JP4780147B2 (en) * | 2008-06-16 | 2011-09-28 | トヨタ自動車株式会社 | Legged robot and walking control method for legged robot |
JP2008229847A (en) * | 2008-07-04 | 2008-10-02 | Toyota Motor Corp | Leg type robot and walking control method of leg type robot |
CN102486648B (en) * | 2010-12-03 | 2013-08-21 | 北京理工大学 | Autonomic mobile robot platform |
EP2688717B1 (en) | 2011-03-23 | 2023-10-18 | SRI International | Dexterous telemanipulator system |
JP5616290B2 (en) * | 2011-05-25 | 2014-10-29 | 本田技研工業株式会社 | Control device for moving body |
JP5560234B2 (en) * | 2011-05-31 | 2014-07-23 | トヨタテクニカルディベロップメント株式会社 | Center of gravity angle estimation method and inverted wheel type traveling body controlled by the method |
JP5962020B2 (en) | 2012-01-17 | 2016-08-03 | セイコーエプソン株式会社 | Robot control apparatus, robot system, robot, and robot control method |
JP5966372B2 (en) | 2012-01-17 | 2016-08-10 | セイコーエプソン株式会社 | Robot control apparatus, robot system, robot control method, and robot |
JP6149791B2 (en) * | 2014-04-24 | 2017-06-21 | トヨタ自動車株式会社 | Center of gravity estimation device and center of gravity estimation method |
JP6732167B2 (en) * | 2016-06-02 | 2020-07-29 | 株式会社ブリヂストン | Joint torque measuring method, inverse dynamics calculation system, and joint torque measuring program |
US10351189B2 (en) | 2016-12-13 | 2019-07-16 | Boston Dynamics, Inc. | Whole body manipulation on a legged robot using dynamic balance |
-
1997
- 1997-12-19 JP JP36441297A patent/JP3672426B2/en not_active Expired - Lifetime
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103895016A (en) * | 2012-12-27 | 2014-07-02 | 现代自动车株式会社 | Method for controlling gait of robot |
Also Published As
Publication number | Publication date |
---|---|
JPH10230485A (en) | 1998-09-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP3672426B2 (en) | Posture control device for legged mobile robot | |
EP0965416B1 (en) | Attitude controller of legged moving robot | |
US6289265B1 (en) | Controller for legged mobile robot | |
EP1514777B1 (en) | Control system of legged mobile robot | |
KR101131775B1 (en) | Controller of leg type moving robot | |
EP1510302B1 (en) | Attitude control device of mobile robot | |
KR101209097B1 (en) | Gait pattern generating device and controller of legged mobile robot | |
US5355064A (en) | Control system for legged mobile robot | |
JP4641252B2 (en) | Gait generator for legged mobile robot | |
JP3629133B2 (en) | Control device for legged mobile robot | |
EP1291137B1 (en) | Floor shape deducing device for legged mobile robot | |
EP1291136A1 (en) | Floor shape deducing device for legged mobile robot | |
JPH05337849A (en) | Attitude stabilization control device for leg type mobile robot | |
KR101766755B1 (en) | Walking robot and control method thereof | |
KR20040068438A (en) | Walking type robot and a moving method thereof | |
JP3167420B2 (en) | Walking control device for legged mobile robot | |
JP3148830B2 (en) | Walking control device for legged mobile robot | |
JPH05245780A (en) | Controller of leg type mobile robot | |
JPH11300661A (en) | Controller for leg-type mobile robot | |
JP3024028B2 (en) | Walking control device for legged mobile robot | |
JP2009279668A (en) | Stabilization controller of legged locomotion robot | |
JP3183557B2 (en) | Walking control device for legged mobile robot | |
JP2009255231A (en) | Walking control device and walking control method | |
JP4237130B2 (en) | Control device for legged mobile robot | |
JPH05277968A (en) | Control device for leg type moving robot |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20041124 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20050124 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20050329 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20050419 |
|
R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20080428 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090428 Year of fee payment: 4 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090428 Year of fee payment: 4 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100428 Year of fee payment: 5 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110428 Year of fee payment: 6 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110428 Year of fee payment: 6 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120428 Year of fee payment: 7 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130428 Year of fee payment: 8 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130428 Year of fee payment: 8 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140428 Year of fee payment: 9 |
|
EXPY | Cancellation because of completion of term |