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

Description

【０００１】
【発明の属する技術分野】
この発明は移動ロボットの姿勢制御装置に関し、特に、腕部を備える２足の脚式移動ロボットあるいは車輪式の移動ロボットの姿勢制御装置において、ロボットが腕を介して対象物から反力を受けるような作業を行なっているときの姿勢安定化を図るようにした姿勢制御装置に関する。
【０００２】
【従来の技術】
移動ロボットの中の脚式移動ロボットの姿勢制御装置としては、本出願人が先に提案した特開平１０−２３０４８５号公報に記載される技術が知られている。その技術においては、対象物から予期せぬ反力を受けると、長期的にはロボットの上体の位置をずらすように姿勢を徐々に修正することによってロボット全体の重心位置をずらし、補償のための重力モーメントを発生させて動バランス、即ち、動力学的に釣り合うバランスを維持すると共に、それまでの過渡期においては、目標床反力中心点（目標ＺＭＰに一致している）まわりに補償のための全床反力モーメントを発生させるように足部を動かして踏み止めさせることによって、動バランスを維持するように構成している。
【０００３】
【発明が解決しようとする課題】
このように、上記した従来技術においては、対象物から受ける予期せぬ反力が急激に変化した場合には、その瞬間では（短期的には）、足部を動かして踏み止めさせることによって動バランスを維持する、即ち、全床反力モーメントを発生させることによって姿勢安定化が図られる。
【０００４】
また、本出願人は、対象物からの反力を受ける受けないに関わらず、ロボットの姿勢が傾いているときに、これを復元させるためのモーメントを全床反力モーメントとして積極的に発生させる技術も提案しているが（特開平１０−２７７９６９号公報）、いずれの技術においても、全床反力モーメントには限界があり、それを越えて全床反力モーメントを発生させようとすると、足部の一部が床から浮いてしまい、その結果、ロボットは動バランスを崩し、最悪の場合には転倒する恐れがあった。
【０００５】
また、本出願人は、全床反力モーメントを発生させる代わりに上体の軌道を修正することによって動バランスを維持する技術も提案しているが（特許第３２６９８５２号公報）、かかる技術にあっては足の着地位置などを修正する必要があるため、腕で対象物に対する作業を行なっているとき、上体や足が作業に適切な位置から離れてしまう場合があった。
【０００６】
また、対象物になんらかの運動を与えるような作業を行なう場合にあっては、作業を所期通り遂行するためには、動バランスを維持したり、あるいは傾いた姿勢の復元を行なっても、対象物の運動に影響を与えないことが望ましい。
【０００７】
従って、この発明の第１の目的は、上記した不都合を解消することにあり、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動バランスを維持して安定な姿勢を保持するようにした移動ロボットの姿勢制御装置を提供することを目的とする。
【０００８】
さらには、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、接地する床から作用される床反力を操作することなく、あるいはその操作量を低減しつつ、動バランスを維持して安定な姿勢を保持するようにした移動ロボットの姿勢制御装置を提供することを第２の目的とする。
【０００９】
さらには、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、対象物の運動に影響を与えることなく動バランスを維持して安定な姿勢を保持するようにした移動ロボットの姿勢制御装置を提供することを第３の目的とする。
【００１０】
【課題を解決するための手段】
上記した目的を達成するために、請求項１項においては、少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも１個の腕部リンクからなる移動ロボットの姿勢制御装置において、前記腕部リンクに実際に作用する対象物からの反力のうちの設定された所定方向の成分である第１の外力を検出あるいは推定する所定方向成分検出手段を備えると共に、前記所定方向成分検出手段が検出あるいは推定した第１の外力に応じ、前記所定方向に直交する方向に前記腕部リンクに第２の外力が作用するように、前記腕部リンクを駆動して前記ロボットの姿勢を安定させるように制御する如く構成した。
【００１１】
移動ロボットの姿勢制御装置において、腕部リンクに実際に作用する対象物からの反力のうちの設定された所定方向の成分である第１の外力に応じ、所定方向に直交する方向に腕部リンクに第２の外力が作用するように、腕部リンクを駆動して前記ロボットの姿勢を安定させるように制御する如く構成したので、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動バランスを維持して安定な姿勢を保持することができる。
【００１２】
請求項２項にあっては、少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも１個の腕部リンクからなる移動ロボットの姿勢制御装置において、少なくとも前記ロボットの目標腕部リンク位置姿勢と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成する目標動作生成手段、前記対象物反力の実際値である実対象物反力を検出あるいは推定する実対象物反力検出手段、少なくとも前記検出あるいは推定された実対象物反力に基づき、前記ロボットの姿勢が安定するように前記目標動作のうちの少なくとも前記目標腕部リンク位置姿勢または前記目標対象物反力を修正する修正する姿勢安定化制御手段、および少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置を備える如く構成した。
【００１３】
移動ロボットの姿勢制御装置において、少なくともロボットの目標腕部リンク位置姿勢と、対象物から腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成し、対象物反力の実際値である実対象物反力を検出あるいは推定し、少なくとも検出あるいは推定された実対象物反力に基づき、ロボットの姿勢が安定するように目標動作のうちの少なくとも目標腕部リンク位置姿勢または目標対象物反力を修正し、修正された目標動作に基づいて腕部リンクを駆動する腕部駆動装置を備える如く構成したので、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動バランスを維持して安定な姿勢を保持することができる。さらには、対象物の運動に影響を与えることなく、動バランスを維持して安定な姿勢を保持することができる。
【００１４】
請求項３項にあっては、前記姿勢安定化制御手段は、少なくとも前記実対象物反力と前記目標対象物反力との差を示す対象物反力偏差に基づき、前記ロボットの姿勢が安定するように前記目標動作を修正する如く構成した。
【００１５】
少なくとも実対象物反力と目標対象物反力との差を示す対象物反力偏差に基づき、ロボットの姿勢が安定するように目標動作を修正する如く構成したので、上記した効果を一層良く得ることができる。
【００１６】
請求項４項にあっては、前記姿勢安定化制御手段は、前記対象物反力偏差の第１の成分によって発生する転倒力の全部あるいは一部を、前記第１の成分から得られる第２の成分によって打ち消すように、前記目標動作を修正する如く構成した。
【００１７】
対象物反力偏差の第１の成分によって発生する転倒力の全部あるいは一部を、第１の成分から得られる第２の成分によって打ち消すように、目標動作を修正する如く構成したので、上記した効果を一層良く得ることができる。
【００１８】
請求項５項にあっては、少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも１個の腕部リンクからなる移動ロボットの姿勢制御装置において、少なくとも前記ロボットの目標腕部リンク位置姿勢と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成する目標動作生成手段、前記ロボットの姿勢傾き偏差を検出し、前記検出した姿勢傾き偏差が零に近づくように前記目標動作のうちの少なくとも前記目標腕部リンク位置姿勢または前記目標対象物反力を修正する姿勢安定化制御手段、および少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置を備える如く構成した。
【００１９】
移動ロボットの姿勢制御装置において、少なくともロボットの目標腕部リンク位置姿勢と、対象物から腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成すると共に、ロボットの姿勢傾き偏差を検出し、検出した姿勢傾き偏差が零に近づくように目標動作のうちの少なくとも目標腕部リンク位置姿勢または目標対象物反力を修正し、修正された目標動作に基づいて腕部リンクを駆動する如く構成したので、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動バランスを維持して安定な姿勢を保持することができる。さらには、対象物の運動に影響を与えることなく、動バランスを維持して安定な姿勢を保持することができる。
【００２０】
尚、特許請求の範囲および発明の詳細な説明欄において、『移動ロボット』は腕部リンク以外に対象物反力を受ける移動ロボットも含む意味で使用する。また、『腕部リンク』に関し、脚部リンクであっても、作業対象物に作用するものであれば、腕部リンクとみなすこととする。例えば、昆虫型の６個の脚部リンクを備えたロボットにおいて、前の２個の脚部リンクを用いて物を持ち上げる場合には、その脚部リンクは腕部リンクとみなすこととする。
【００２１】
また、「位置姿勢」は「位置」および「姿勢」の両者を含んでも良く、あるいはそのいずれかであっても良い意味で使用する。換言すれば、「位置姿勢」は、必ず「位置」および「姿勢」の両者を含まなければならないものではない。
【００２２】
【発明の実施の形態】
以下、添付図面を参照してこの発明の一つの実施の形態に係る移動ロボットの姿勢制御装置を説明する。
【００２３】
図１はこの実施の形態に係る移動ロボットの姿勢制御装置が対象とする脚式移動ロボットの正面図、図２はその側面図である。尚、移動ロボットとしては、２個の脚部リンクと２個の腕部リンクを備えたヒューマノイド型（人間型）の脚式移動ロボットを例にとる。
【００２４】
図１に示すように、脚式移動ロボット（以下「ロボット」という）１は、複数個（本）、より具体的には２個（本）の脚部リンク（あるいは脚）２を備えると共に、その上方には上体（基体）３が設けられる。上体３のさらに上方には頭部４が形成されると共に、上体３の両側には２個（本）の腕部リンク（あるいは腕）５が連結される。また、図２に示すように、上体３の背部には格納部６が設けられ、その内部には電子制御ユニット（後述）などが収容される。尚、図１および図２に示すロボット１は、内部構造を保護するためのカバーで被覆される。
【００２５】
図３はロボット１をスケルトンで示す説明図である。同図を参照してその内部構造を関節を中心に説明すると、図示の如く、ロボット１は、左右それぞれの脚部リンク２および腕部リンク５に、１１個の電動モータで動力化された６個の関節を備える。
【００２６】
即ち、ロボット１は、腰部（股部）に、脚部リンク２を鉛直軸（Ｚ軸あるいは鉛直軸）まわりに回転させる関節を駆動する電動モータ１０Ｒ，１０Ｌ（右側をＲ、左側をＬとする。以下同じ）と、脚部リンク２をピッチ（進行）方向（Ｙ軸まわり）に揺動させる関節を駆動する電動モータ１２Ｒ，１２Ｌと、脚部リンク２をロール（左右）方向（Ｘ軸まわり）に回転させる関節を駆動する電動モータ１４Ｒ，１４Ｌを備えると共に、膝部に脚部リンク２の下部をピッチ方向（Ｙ軸まわり）に回転させる膝関節を駆動する電動モータ１６Ｒ，１６Ｌを備え、さらに足首に脚部リンク２の先端側をピッチ方向（Ｙ軸まわり）に回転させる足（足首）関節を駆動する電動モータ１８Ｒ，１８Ｌとロール方向（Ｘ軸まわり）に回転させる足（足首）関節を駆動する電動モータ２０Ｒ，２０Ｌを備える。
【００２７】
上記したように、図３において、関節はそれを駆動する電動モータ（あるいは電動モータに接続されてその動力を伝動するプーリなどの伝動要素）の回転軸線で示す。尚、脚部リンク２の先端には足部（足平）２２Ｒ，２２Ｌが取着される。
【００２８】
このように、脚部リンク２の股関節（腰関節）には電動モータ１０Ｒ（Ｌ），１２Ｒ（Ｌ），１４Ｒ（Ｌ）がそれらの回転軸線が直交するように配置されると共に、足関節（足首関節）には電動モータ１８Ｒ（Ｌ），２０Ｒ（Ｌ）がそれらの回転軸線が直交するように配置される。尚、股関節と膝関節は大腿リンク２４Ｒ（Ｌ）で、膝関節と足関節は下腿リンク２６Ｒ（Ｌ）で連結される。
【００２９】
脚部リンク２は股関節を介して上体３に連結されるが、図３では上体３を上体リンク２８として簡略的に示す。前記したように、上体３には腕部リンク５が連結される。
【００３０】
腕部リンク５も、脚部リンク２と同様に構成される。即ち、ロボット１は、肩部に、腕部リンク５をピッチ方向に回転させる関節を駆動する電動モータ３０Ｒ，３０Ｌとロール方向に回転させる関節を駆動する電動モータ３２Ｒ，３２Ｌを備えると共に、その自由端側を回転させる関節を駆動する電動モータ３４Ｒ，３４Ｌと、肘部にそれ以降の部位を回転させる関節を駆動する電動モータ３６Ｒ，３６Ｌを備え、さらにその先端側にそれを回転させる手首関節を駆動する電動モータ３８Ｒ，３８Ｌを備える。尚、手首の先にはハンド（エンドエフェクタ）４０Ｒ，４０Ｌが取着される。
【００３１】
即ち、腕部リンク５の肩関節には電動モータ３０Ｒ（Ｌ），３２Ｒ（Ｌ），３４Ｒ（Ｌ）がそれらの回転軸線が直交するように配置される。尚、肩関節と肘関節とは上腕リンク４２Ｒ（Ｌ）で、肘関節と手首関節とは下腕リンク４４Ｒ（Ｌ）で連結される。
【００３２】
また、頭部４は、鉛直軸まわりの首関節４６と、それと直交する軸まわりに頭部４を回転させる頭部揺動機構４８を介して上体３に連結される。図３に示す如く、頭部４の内部には撮像した画像を示す信号を出力する、ＣＣＤカメラからなる視覚センサ５０が配置されると共に、レシーバおよびマイクロフォンからなる音声入出力装置５２が配置される。
【００３３】
上記の構成により、脚部リンク２は左右の足について６個の関節を備えて合計１２の自由度を与えられ、６個の関節を適宜な角度で駆動（関節変位）することで、脚部リンク２に所望の動きを与えることができ、ロボット１を任意に３次元空間を歩行させることができる。また、腕部リンク５も左右の腕について５個の関節を備えて合計１０の自由度を与えられ、５個の関節を適宜な角度で駆動（関節変位）することで所望の作業を行わせることができる。さらに、頭部４は２つの自由度からなる関節あるいは揺動機構を与えられ、これらを適宜な角度で駆動することにより所望の方向に頭部４を向けることができる。
【００３４】
１０Ｒ（Ｌ）などの電動モータのそれぞれにはロータリエンコーダ（図示せず）が設けられ、電動モータの回転軸の回転を通じて対応する関節の角度、角速度、および角加速度の少なくともいずれかを示す信号を出力する。
【００３５】
足部２２Ｒ（Ｌ）には公知の６軸力センサ（以下「力センサ」という）５６が取着され、ロボットに作用する外力の内、接地面からロボット１に作用する床反力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚを示す信号を出力する。
【００３６】
また、手首関節とハンド４０Ｒ（Ｌ）の間には同種の力センサ（６軸力センサ）５８が取着され、ロボット１に作用する床反力以外の外力、具体的にはハンド４０Ｒ（Ｌ）に対象物から作用する外力（対象物反力）の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚを示す信号を出力する。
【００３７】
さらに、上体３には傾斜センサ６０が設置され、鉛直軸に対する上体３の傾き（傾斜角度）とその角速度の少なくともいずれか、即ち、ロボット１の上体３の傾斜（姿勢）などの状態量を示す信号を出力する。
【００３８】
これら力センサ５６などの出力群は、ロボット１の上体３の背中側の格納部６（図２に示す）に収容されたマイクロコンピュータからなる電子制御ユニットElectric Control Unit 。以下「ＥＣＵ」という）６２に送られる（図示の便宜のためロボット１の右側についてのみ図示する）。
【００３９】
図４はＥＣＵ６２の詳細を示すブロック図であり、マイクロ・コンピュータから構成される。そこにおいて傾斜センサ６０などの出力はＡ／Ｄ変換器７０でデジタル値に変換され、その出力はバス７２を介してＲＡＭ７４に送られる。また各アクチュエータにおいて電動モータに隣接して配置されるエンコーダの出力は、カウンタ７６を介してＲＡＭ７４内に入力される。
【００４０】
ＥＣＵ６２内にはＣＰＵからなる演算装置８０が設けられており、演算装置８０が生成した歩容に基づいてロボットが安定な姿勢を継続することができるように、関節角変位指令（アクチュエータ変位指令）を算出し、ＲＡＭ７４に送出する。尚、符号８４はＲＯＭを示す。
【００４１】
また、演算装置８０はＲＡＭ７４からその指令と検出された実測値とを読み出し、各関節の駆動に必要な制御値（操作量）を算出してＤ／Ａ変換器８６と各関節に設けられたアクチュエータ駆動装置（アンプ）８８を介して各関節を駆動する、変位検出器（エンコーダ）付きの脚アクチュエータ９０と腕アクチュエータ９２の電動モータ１０Ｒ（Ｌ），１２Ｒ（Ｌ）などに出力する。
【００４２】
図５は、この実施の形態に係る脚式移動ロボットの姿勢制御装置（主として前記した演算装置８０に相当）の構成および動作を機能的に示すブロック図である。
【００４３】
この装置は脚および腕の動作を統合的に制御する装置であり、後述するように操作量を算出して各アクチュエータ駆動装置８８にアクチュエータ変位指令を出力し、脚アクチュエータ９０と腕アクチュエータ９２を動作させる。図示の如く、この装置は、目標作業パターン生成器１００、脚メイン制御装置１０２、腕メイン制御装置１０４、および姿勢安定化メイン制御装置１０６などから構成される。
【００４４】
目標作業パターン生成器１００は、ある想定条件下においてロボット１の動力学的平衡条件を満足する歩容を含む、目標作業パターンを生成する。目標作業パターンは、複数の変数の時間変化パターンによって表現される。この変数は、運動を表現する変数と環境から受ける反力を表現する変数から構成される。
【００４５】
ここで、運動を表現する変数は、これによって各瞬間における姿勢が一義的に決定できる変数の組である。具体的には、目標足部位置姿勢、目標上***置姿勢、目標ハンド位置姿勢から構成される。
【００４６】
また、環境から受ける反力を表現する変数は、後述する目標全床反力中心点、目標全床反力および目標対象物反力から構成される。
【００４７】
これら各変数は、支持脚座標系で表される。支持脚座標系は、支持脚足首（足関節を駆動する電動モータ１８，２０Ｒ（Ｌ）の回転軸線の交点）から足部２２Ｒ（Ｌ）への垂直投影点を原点とする座標系であり、図６および図７に示すように、支持脚が接地している床に固定された座標系であり、支持脚足部の前向きをＸ軸の向き、左向きをＹ軸の向き、鉛直方向上向きをＺ軸向きとする座標系である。
【００４８】
以下、これら各変数を詳細に説明する。
【００４９】
先ず、「対象物反力」とは、ロボット１が環境から受ける外力の内で、各足部床反力を除いた外力を意味する。具体的には、ハンド４０Ｒ（Ｌ）が作業対象物（例えばドアの把手）から受ける反力を意味する。尚、その目標値を「目標対象物反力」と呼ぶ。
【００５０】
目標作業パターン生成器１００が出力する目標対象物反力は、後述する目標全床反力中心点まわりに作用する力とモーメントによって表現される。ちなみに、姿勢安定化にとって重要なのは、このうちのモーメント成分である。
【００５１】
目標全床反力と目標全床反力中心点（位置）について説明すると、作業中において各足部が床から受けるべき目標床反力の合力を「広義の目標全床反力」と呼ぶ。広義の目標全床反力は、作用点とその点における力とモーメントで表現される。目標全床反力中心点（位置）は、目標全床反力をその点を作用点とする力とモーメントで表現したとき、Ｘ軸まわりモーメント成分とＹ軸まわりモーメント成分が零（０）になる床面上の点である。
【００５２】
狭義の目標全床反力は、広義の目標全床反力を、目標全床反力中心点を作用点として、力とモーメントで表現した場合の力とモーメントを意味する。目標作業パターン生成器が出力する目標全床反力は、狭義の目標全床反力である。
【００５３】
以降は特に説明がない限り、目標全床反力は、狭義の目標全床反力を指す。尚、平坦な床面を歩行する場合には、目標全床反力中心点は、通常、その床面上に設定される。
【００５４】
歩行制御の分野において従来から公知である、Ｍ．Ｖｕｋｏｂｒａｔｏｖｉｃによって提唱されたＺＭＰの概念も、次のように拡張する。即ち、ロボット１の運動によって生じる慣性力と重力と対象物反力の合力が、その点を作用点とする力とモーメントで表現されたとき、Ｘ軸まわりモーメント成分とＹ軸まわりモーメント成分が０になる床面上の点を「ＺＭＰ」と呼ぶ。ロボット１が目標の運動を行うときのＺＭＰを「目標ＺＭＰ」と呼び、その位置を「目標ＺＭＰ位置」と呼ぶ。
【００５５】
目標作業パターンが動力学的平衡条件を満足するということは、目標作業パターンによって生じる上記の慣性力と重力と対象物反力の合力と目標全床反力が、打ち消し合って零になることである。従って、動力学的平衡条件を満足するためには、目標全床反力中心点（位置）と目標ＺＭＰ（位置）が一致しなければならない。
【００５６】
目標作業パターン生成器１００では、後述する大局安定化制御が働いていない場合、動力学的平衡条件を満足する歩容を含む目標作業パターンを生成する。換言すれば、この場合、目標作業パターン生成器１００が生成する目標全床反力中心点（位置）は目標ＺＭＰ（位置）に一致する。
【００５７】
目標足部位置姿勢、目標上***置姿勢、目標ハンド位置姿勢は、前記した支持脚座標系で表現されたそれぞれの部位の位置と姿勢を表す。具体的には、上体３の位置およびその速度は、上体３の重心位置などの代表点およびその（変位）速度を意味する。更に、上体３あるいは足部２２Ｒ（Ｌ）の姿勢は、Ｘ，Ｙ，Ｚ空間における向きを意味する。
【００５８】
次いで、脚メイン制御装置１０２について説明する。脚メイン制御装置１０２の機能は、概説すると、脚アクチュエータ（１０Ｒ（Ｌ）などの電動モータなど）を操作し、目標姿勢に追従する姿勢追従制御と、目標全床反力と補償全床反力モーメント（後述）の合力に追従する床反力制御を同時に行う装置である。尚、目標床反力と補償全床反力モーメントの合力と目標姿勢を同時に完全に満足させることは不可能であるので、適当な調整が行われ、長期的には両方を満足するように制御される。
【００５９】
より詳しくは、脚メイン制御装置１０２は、修正目標上***置姿勢（後述）、目標足部位置姿勢、目標全床反力中心点（位置）と、その中心点に作用する目標全床反力と、補償全床反力モーメントを入力し、目標全床反力中心点（位置）に作用する実全床反力のモーメント成分が、目標全床反力モーメント（通常は零）と補償全床反力モーメントの和に一致するように、目標足部位置姿勢を修正する。修正された目標足部位置姿勢を「修正目標足部位置姿勢」と呼ぶ。
【００６０】
脚メイン制御装置１０２は、この修正目標上***置姿勢と修正目標足部位置姿勢から決定される目標脚関節変位に実関節変位が追従するように、アクチュエータ変位指令を出力して脚アクチュエータ９０を制御する。
【００６１】
以上の機能を実現させるために、この実施の形態にあっては、前記した特開平１０−２７７９６９号公報に記載される技術（以下「複合コンプライアンス制御」という）を適用する。この複合コンプライアンス制御を行う装置は、脚メイン制御装置の他に、足部２２Ｒ（Ｌ）に設けた力センサ５６、アクチュエータ駆動装置８８およびアクチュエータ９０を包含して構成される。
【００６２】
図８に複合コンプライアンス制御装置のブロック図を示すが、その詳細な説明は上記特開平１０−２７７９６９号公報に記載されているので省略する。尚、図８に示す複合コンプライアンス制御装置においては、特開平１０−２７７９６９号公報に記載される複合コンプライアンス制御装置と比較すると、上体傾斜フィードバック系の姿勢安定化制御（補償全床反力モーメントの演算）が省かれている。
【００６３】
その理由は、同様の処理が後述の姿勢安定化メイン制御装置で行なわれ、そこで求められた補償全床反力モーメントが入力されるためである。但し、上体傾斜偏差による床反力の変動を抑制するため、上体傾斜フィードバック系を追加しても良い。
【００６４】
また、この実施の形態において複合コンプライアンス制御に、上記した特許第３２６９８５２号公報に記載される技術（以下、「大局安定化制御」という）を加えることとする。但し、大局安定化制御は上体の位置や歩幅を修正、より具体的には上記した修正目標上***置姿勢をさらに修正するので、後述の腕部リンク５の制御にあたり、ハンド４０Ｒ（Ｌ）と対象物との相対位置関係が重要な場合には、修正される上体の位置や歩幅の影響を考慮する必要がある。また、大局安定化制御が働くと、目標作業パターンにおける目標ＺＭＰは、目標全床反力中心点から故意にずらされた点に制御される。
【００６５】
大局安定化制御が加えられることで修正目標上***置姿勢がさらに修正されることから、それによって最終的に得られる目標上***置姿勢を、図５に示す如く、「最終修正目標上***置姿勢」と呼ぶ。尚、大局安定化制御を加えることはこの発明において必須ではない。即ち、大局安定化制御を加えなくても良く、その場合は、修正目標上***置姿勢を最終修正目標上***置姿勢として扱えば足りる。
【００６６】
尚、図５において、脚メイン制御装置１０２で修正された最終修正目標足部位置姿勢は、姿勢安定化メイン制御装置１０６に入力される。但し、姿勢安定化メイン制御装置１０６において、目標足部位置姿勢が修正されたことによるロボットの重心位置の変化が無視できるならば、最終修正目標足部位置姿勢を姿勢安定化メイン制御装置１０６に入力させる必要はない。
【００６７】
次いで腕メイン制御装置１０４について説明する。腕メイン制御装置１０４の機能は、概説すると、腕アクチュエータ（３０Ｒ（Ｌ）などの電動モータなど）９２を操作し、目標姿勢に追従する姿勢制御と修正目標対象物反力（後述）に追従する対象物反力制御を同時に行うことである（ここでの「姿勢」は腕部リンク５の全関節の変位の組を表す）。目標姿勢と修正目標対象物反力を同時に完全に満足させることは不可能であるので、適宜な手法、例えば、従来からマニピュレータのコンプライアンス制御、いわゆる仮想コンプライアンス制御として知られるものを用いる（機械工学便覧、エンジニアリング編、Ｃ４−１００頁）。
【００６８】
具体的な制御系構成とアルゴリズムを以下に説明すると、腕メイン制御系は腕メイン制御装置１０４の他に、前記したハンド４０Ｒ（Ｌ）に設けた力センサ５８、アクチュエータ駆動装置８８および腕アクチュエータ９２を包含して構成される。
【００６９】
腕メイン制御装置１０４は、最終目標上***置姿勢（あるいは修正目標上***置姿勢）、目標ハンド位置姿勢および修正目標対象物反力を入力し、力センサ５８によって検出される実対象物反力と修正目標対象物反力の差に応じて目標ハンド位置姿勢を修正する。修正された目標ハンド位置姿勢を「最終修正目標ハンド位置姿勢」と呼ぶ。そして、最終修正目標上***置姿勢（あるいは修正目標上***置姿勢）と最終修正目標ハンド位置姿勢から決定される目標腕関節変位に実関節変位が追従するように腕アクチュエータ９２を制御する。
【００７０】
次いで姿勢安定化メイン制御装置１０６について説明する。姿勢安定化メイン制御装置１０６は、動バランスあるいは姿勢バランスをとるために動力学的平衡条件を考慮しながら制御を行う。そこで、先ず、装置の概要を説明する前に、動力学的平衡条件について以下に説明する。
【００７１】
実際のロボット１の姿勢傾きの挙動を決定する最も大きな要因は、目標全床反力中心点（即ち、目標ＺＭＰ）まわりでの実際の力のモーメントのバランスである。
【００７２】
目標全床反力中心点まわりに作用する力のモーメントを以下に列挙する。
１）慣性力モーメント
２）重力モーメント
３）全床反力モーメント
４）対象物反力モーメント
【００７３】
慣性力モーメントは、目標全床反力中心点まわりのロボットの角運動量の変化によって生じるモーメントである。この値はオイラー方程式によって求められ、具体的には目標全床反力中心点まわりのロボット１の角運動量の１階微分値の符号を反転させたものである。目標作業パターンの慣性力モーメントを「目標慣性力モーメント」と呼ぶ。実際のロボット１が作業しているときの慣性力モーメントを「実慣性力モーメント」と呼ぶ。重力モーメントは、ロボット１の重心に作用する重力が目標全床反力中心点まわりに作用するモーメントである。
【００７４】
２個の足部２２Ｒ（Ｌ）にそれぞれ作用する床反力の合力を「全床反力」と呼ぶ。全床反力モーメントは、全床反力が目標全床反力中心点まわりに作用するモーメントである。
【００７５】
前述のように、ハンド４０Ｒ（Ｌ）が対象物から受ける反力を「対象物反力」と呼ぶ。対象物反力モーメントは、対象物反力が目標全床反力中心点まわりに作用するモーメントである。
【００７６】
さて、理想的な脚メイン制御装置により、ロボット１が目標作業パターンの運動パターンに忠実に追従していたと仮定する。このときには実慣性力モーメントは目標慣性力モーメントに一致し、実重力モーメントは目標重力モーメントに一致する。
【００７７】
一方、動力学の法則（オイラー方程式）により、実慣性力モーメントと実重力モーメントと実全床反力モーメントと実対象物反力モーメントの和は、必ず零である。
【００７８】
故に、ロボット１が忠実に目標作業パターンの運動パターン通りに動くためには、目標慣性力モーメントと目標重力モーメントと実全床反力モーメントと実対象物反力モーメントの和は、零でなければならない。これを条件１とする。
【００７９】
ところが、実際には実対象物反力モーメントが目標対象物反力モーメントと一致せず、差が生じる。例えば、図９に示すように、台車（対象物）１０８を押す作業を行っているとき、台車１０８の実際のころがり摩擦力の絶対値が想定していた値よりも突然小さくなってしまった状況である。
【００８０】
この状況では、実対象物反力が目標全床反力中心点のＹ軸まわりに作用するモーメントは、目標対象物反力が目標全床反力中心点のＹ軸まわりに作用するモーメントよりも正の向きに大きくなって条件１を満たさなくなり、ロボット１は前傾する。尚、モーメントの向きは、座標軸の正方向に向いてロボット１を時計まわりに回転させるモーメントを正とする。
【００８１】
このような状況においても条件１を満足させるためには、先に提案した技術では次の２通りの手法が考えられた。
【００８２】
手法１）上記偏差を打ち消すように、実全床反力モーメントを変える。具体的には、目標全床反力中心点まわりに負の床反力モーメントを発生するように脚メイン制御装置１０２に指令し、脚メイン制御装置１０２においてアクチュエータ変位指令を行って足部２２Ｒ（Ｌ）のつまさきを下げ、実全床反力モーメントを負の向きに増加させる、即ち、脚部リンク２で踏み止めさせるような姿勢をとらせる。
【００８３】
手法２）上記偏差を打ち消すように、目標作業パターンの運動パターンを修正することにより、目標慣性力モーメントと目標重力モーメントを修正する。具体的には、目標上***置および／または姿勢を修正することにより、目標慣性力モーメントと目標重力モーメントを修正する。即ち、上体３を前後方向に移動させる。
【００８４】
実全床反力モーメントは、目標全床反力モーメントを変えるだけで脚メイン制御装置によってすばやく変化させることができるので、手法１は短期的な対応に向いている。但し、実全床反力モーメントを大きく変化させると、足部２２Ｒ（Ｌ）の接地圧分布が偏って接地感が減少し、最悪の場合には足部２２Ｒ（Ｌ）の一部が浮いてしまう。従って、長期的には、なるべく元の目標全床反力モーメントに戻すべきである。
【００８５】
実全床反力モーメントを元の目標全床反力モーメントに戻すためには、重心位置をずらし（この場合は後方にずらし）、目標重力モーメントによって上記偏差を打ち消すように、手法２によって目標作業パターンの運動パターンを修正すれば良い。但し、重心位置を急激にずらすと、過大な目標慣性力モーメントが逆向きに発生するので、ゆっくりと重心位置をずらす必要がある。従って、手法２は長期的な対応に向いている。
【００８６】
従来技術で述べた、本出願人が先に提案した特開平１０−２３０４８５号公報記載の技術は、これら両方の手法を同時に行い、短期的には手法１を主に使うことによって速い変化に対応し、長期的には手法２を主に使うことによって実全床反力モーメントを元の目標全床反力モーメントに収束させるものであった。
【００８７】
これに対し、この発明に係る姿勢制御装置においては、さらに、実対象物反力の成分の内の拘束方向成分を意図的に変化させる手法（手法３）を追加するようにした。
【００８８】
以下、手法３の原理について説明する。先ず、理解の便宜のため、１個（本）の腕部リンクを備えた脚式移動ロボットが、その腕部リンクの先端に配置されたハンドを用いてある対象物に対して作業を行っている場合を考える。このとき、ハンドは対象物から拘束力を受け、拘束運動をするものとする。
【００８９】
対象物がハンドによって空中に支持され、ハンド以外のものによって支持されないならば、ハンドの運動には３次元の平行移動の自由度と３次元の回転の自由度の合計６つの自由度があり、対象物からは何等拘束力を受けない。
【００９０】
これに対し、対象物がハンド以外のものからも支持されるならば、場合によっては、ハンドの運動はハンド以外の支持されるものによって拘束力を受ける。具体的には、ハンドが対象物、例えば静止した剛性の高い表面に接触すると、ハンドはその面を貫いて動くことはできず、故に自然な位置の拘束が生じる、換言すれば、その面から拘束力を受けることとなる。この発明に係る姿勢制御装置は、その拘束力に着目し、ロボットの姿勢安定化を図る制御を行うようにした。
【００９１】
説明を続ける前に、ここで、説明に用いる座標系、各種空間や方向を以下のように定義する。尚、以下に記載する変数は、特に、ことわりがない限り、支持脚座標系で表されるものとする。
【００９２】
また、ハンドのある瞬間におけるＸ，ＹおよびＺ方向への速度成分をそれぞれＶｘ，Ｖｙ，Ｖｚとする。同じ瞬間におけるＸ，ＹおよびＺ軸まわりの回転速度成分をそれぞれωｘ，ωｙ，ωｚとする。回転は、座標軸の正の方向に向いて時計まわりに回転する向きを正とする。
【００９３】
この瞬間におけるハンドの速度を、ベクトル（Ｖｘ，Ｖｙ，Ｖｚ，ωｘ，ωｙ，ωｚ）で表わすこととし、これを「ハンド速度ベクトル」と呼ぶ。
【００９４】
そして、任意の実数Ｖｘ，Ｖｙ，Ｖｚ，ωｘ，ωｙ，ωｚを要素とするハンド速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ，ωｘ，ωｙ，ωｚ）の集合を「全運動速度空間」と呼ぶ。全運動速度空間は、いわゆるベクトル空間になっている。
【００９５】
また、ある瞬間に実現不可能なハンド速度ベクトル全ての集合を、この瞬間の「拘束速度領域」と呼ぶ。ここでいう実現不可能なハンド速度ベクトルは、幾何学的な観点から判定される。つまり、この瞬間にハンドと対象物の間の干渉により、ほとんどハンドの変位を生じることなく応力が発生するハンド速度ベクトルを、実現不可能なハンド速度ベクトルと定義する。このとき、腕アクチュエータや腕の関節配置などに因る制約は考えない。
【００９６】
即ち、ハンド自身は、対象物がなければ任意の速度で移動可能であることを前提とする。また、このとき、ハンドおよび対象物が十分な剛性を備えていれば、剛体とみなす。従って、あるハンド速度ベクトル（Ｖｘ１，Ｖｙ１，Ｖｚ１，ωｘ１，ωｙ１，ωｚ１）が拘束速度領域の要素ならば、任意の正の実数ｋを乗じた（ｋＶｘ１，ｋＶｙ１，ｋＶｚ１，ｋωｘ１，ｋωｙ１，ｋωｚ１）も拘束速度領域の要素とする。尚、便宜上、零ベクトル（０，０，０，０，０，０）も拘束速度領域の要素とする。
【００９７】
ここで注意すべき点は、拘束速度領域を、拘束条件下での移動可能な速度領域と混同しないことである。また、拘束速度領域は、いわゆるベクトル空間とは限らない。例えば、床面に載置された十分に硬い対象物をハンドで把持した場合、ハンドを真下（床面の法線方向下方）に下げることはできないが、逆方向（法線方向上方）には移動することができる。
【００９８】
つまり、真下方向へのハンド速度ベクトルは拘束速度領域の要素であるが、その逆方向は拘束速度領域の要素になっていない。拘束速度領域がベクトル空間であれば、拘束速度領域の任意の要素（Ｖｘ１，Ｖｙ１，Ｖｚ１，ωｘ１，ωｙ１，ωｚ１）に対し、逆ベクトル（−Ｖｘ１，−Ｖｙ１，−Ｖｚ１，−ωｘ１，−ωｙ１，−ωｚ１）も拘束速度領域の要素でなければならないが、この場合の拘束速度領域は、この条件を満足しないので、ベクトル空間ではない。
【００９９】
次に、拘束速度空間を以下のように定義する。即ち、拘束速度領域の任意のベクトル（Ｖｘ１，Ｖｙ１，Ｖｚ１，ωｘ１，ωｙ１，ωｚ１）と逆ベクトル（−Ｖｘ１，−Ｖｙ１，−Ｖｚ１，−ωｘ１，−ωｙ１，−ωｚ１）の集合を、拘束速度空間と定義する。通常は、拘束速度空間は、全運動速度空間の部分ベクトル空間になっている。
【０１００】
全運動速度空間内のベクトルの内で、拘束速度領域の任意のベクトルとの内積が零である（即ちそのベクトルと直交する）全てのベクトルの集合を「自由速度空間」と呼ぶ。
【０１０１】
ここで、拘束速度空間および自由速度空間内のベクトルを次のように定義する。拘束速度空間の要素である方向ベクトルを「拘束方向ベクトル」と呼び、単に拘束方向というときは、任意の拘束方向ベクトルの向きを指す。また、自由速度空間の要素である方向ベクトルを「自由方向ベクトル」と呼び、単に自由方向というときは、任意の自由方向ベクトルの向きを指す。
【０１０２】
以下にいくつかの対象物に対する作業に関して拘束速度空間を示す。
【０１０３】
作業１）対象物をハンドによって空中に持ち上げている場合
【０１０４】
この場合、拘束速度空間は存在しない。尚、便宜上、拘束速度空間は零ベクトルだけからなる空間とする。自由速度空間は全運動速度空間に一致する。
【０１０５】
作業２）前記した図９に示す如く、自在キャスタ輪が付いた台車１０８をハンド４０Ｒで把持して押す場合
【０１０６】
この場合、拘束速度空間は、任意の実数Ｖｚ，ωｘ，ωｙを持つハンド速度ベクトル（０，０，Ｖｚ，ωｘ，ωｙ，０）の集合である。
【０１０７】
作業３）図１０に示す如く、ハンド４０Ｒで把手１１０を把持しながら、ヒンジ１１２が付いたドア１１４を開閉する場合
【０１０８】
この場合、ヒンジ軸１１２Ｚまわりの回転以外の運動は拘束される。ヒンジ軸をＺ軸とし、この瞬間のハンドのＸ座標を０、Ｙ座標を−ｒ（ｒはドア１１４の回転半径、より詳しくは把手１１０の回転半径）とすると、自由速度空間は、任意の実数Ｖｘを持つハンドの速度ベクトル（Ｖｘ，０，０，０，０，Ｖｘ／ｒ）の集合である。
【０１０９】
また、拘束速度空間は、任意の実数Ｖｘ，Ｖｙ，Ｖｚ，ωｘ，ωｙを持つハンド速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ，ωｘ，ωｙ，−ｒ＊Ｖｘ）の集合である。
【０１１０】
作業４）図１１に示すような環境において、円柱状の手摺り１２０にハンド４０Ｒを滑らせながら階段１２２を昇降する場合
【０１１１】
この場合、手摺り１２０の軸方向の平行移動と、手摺り１２０の軸まわりの回転以外の運動は拘束される。手摺り１２０の軸が式ｘ＝ｚ＋ｃ，ｙ＝−ｄ（ｃおよびｄは定数）で表わされるとすると、自由速度空間は、任意の実数Ｖｘおよびωｘを持つハンド速度ベクトル（Ｖｘ，０，Ｖｘ，ωｘ，０，ωｘ）である。また、拘束速度空間は、任意の実数Ｖｘ，Ｖｙ，ωｘ，ωｙを持つハンド速度ベクトル（Ｖｘ，Ｖｙ，−Ｖｘ，ωｘ，ωｙ，−ωｘ）の集合である。尚、理解の便宜のため、手摺り１２０とＸ軸のなす角度を４５度とする。
【０１１２】
作業５）図１１に示す環境において、手摺り１２０をハンド４０Ｒでしっかり把持して階段１２２を昇降する場合
【０１１３】
手摺り１２０をしっかりと把持しているときには運動の自由度はなく、全ての運動が拘束される。従って、拘束速度空間は、全運動速度空間に一致する。
【０１１４】
上記のような作業において、ハンドから対象物に対してハンドの拘束方向に力を作用させたとき、対象物にはハンド以外の支持するものからその力を打ち消す拘束力しか発生せず、自由方向に実質的に摩擦力などが発生しないならば、ハンドおよび対象物の運動は変化しない。
【０１１５】
作業３（図１０）を例にとると、ハンド４０Ｒによってドア１１４に上向き（Ｚ軸上方）の力を作用させても、ヒンジ１１２の摩擦が実質的に零であるならば、ヒンジ１１２からドア１１４に前記上向きの力を打ち消す力だけが作用するので、ドア１１４の回転運動に何の影響も与えない。
【０１１６】
この発明に係る姿勢制御装置は、かかる性質を利用することで、対象物の運動制御に干渉せずに、ロボットの姿勢を安定化しようとするものである。即ち、対象物からハンドに作用する力の拘束方向成分を制御することにより、対象物の運動に影響を与えることなく、ロボットの動バランスを維持したり、傾いた姿勢を復元するように構成した。
【０１１７】
以下に、作業３を例にとり、この作用について説明する。前述した図１０に示すように、ロボット１がドア１１４の前に立ってその開閉作業を行っているとき、ロボット１が前に倒れそうになった状況を考える。ロボット１の姿勢を復元させるための一つの方法は、ドア１１４を前に押すことである。これによってロボット１はドア１１４から反力を受け、姿勢が復元する。
【０１１８】
しかし、姿勢安定化制御のためにドア１１４を押してしまうと、ドア１１４は加速され、その運動は望みの開閉運動からずれてしまうので、あまり大きな復元力を発生させることはできない。特に、ロボット１の質量に較べてドアの質量が非常に小さい場合には、ほとんど復元力を発生させることができない。また、ドア１１４の運動が所期の予定から変更されるため、作業遂行上望ましくない。
【０１１９】
ロボット１の姿勢を復元させるためのもう一つの方法は、ドア１１４を下に押すことである。これによってロボット１はドア１１４から反力を受け、姿勢が復元する。さらに、拘束方向に押しているだけなので、前述のようにドアの運動は変わらない。
【０１２０】
この発明に係る姿勢制御装置は後者の方法を用いるものであるため、前記作業２、作業３および作業４のようにハンドの運動に何らかの拘束方向が存在することが、この発明を適用するための必要条件になる。従って、前記作業１に対しては適用することができない。
【０１２１】
尚、拘束速度空間などを表す座標系は、円筒座標系や極座標系などでも良い。通常の座標系なら何を用いてもそれらの間で変換も可能であると共に、その瞬間における拘束速度空間が指す空間に相違はないためである。作業に応じて座標系を適宜設定することで、作業中、拘束速度空間を一定（普遍）にすることができる場合もある。例えば、前記作業３では、円筒座標系を用いてヒンジ１１２の軸を円筒座標系の軸に一致させることで、各瞬間における拘束速度空間を一定に保つことができる。
【０１２２】
次いで、ｎ個（本）の腕を備えるロボットが作業を行う場合について考える。この場合、各種空間や方向の定義を、次のように拡張する。
【０１２３】
まず、下式のように、各ハンドに１からｎまでの識別番号をつけ、それぞれを「第ｊハンド」（ｊ＝１，２，・・・，ｎ）と呼んで区別する
【０１２４】
座標系は前記した支持脚座標系を用いることとし、第ｊハンドのある瞬間におけるＸ，ＹおよびＺ方向への速度成分を、それぞれＶｘｊ，Ｖｙｊ，Ｖｚｊとする。また、同じ瞬間におけるＸ，ＹおよびＺ軸まわりの回転速度成分を、それぞれωｘｊ，ωｙｊ，ωｚｊとする。
【０１２５】
そして、ベクトル（Ｖｘ１，Ｖｙ１，Ｖｚ１，ωｘ１，ωｙ１，ωｚ１，Ｖｘ２，Ｖｙ２，Ｖｚ２，ωｘ２，ωｙ２，ωｚ２，・・・，Ｖｘｎ，Ｖｙｎ，Ｖｚｎ，ωｘｎ，ωｙｎ，ωｚｎ）を「全ハンド速度ベクトル」と呼ぶ。このベクトルは、第１ハンドから第ｎハンドまでのハンド速度ベクトルの各要素を順に並べたものである。尚、このベクトル表現の代りに、ｊ行目が第ｊハンドの速度ベクトルになっている、ｎ行６列の行列で表わしても良い。
【０１２６】
以降、全ハンド速度ベクトルを改めて「ハンド速度ベクトル」と呼ぶ。また、任意の実数を要素とするハンド速度ベクトルの集合を、改めて「全運動速度空間」と呼ぶ。この全運動速度空間は、６＊ｎ次元ベクトル空間になっている。
【０１２７】
さらに、ある瞬間に実現不可能なハンド速度ベクトル全ての集合を、改めてこの瞬間の「拘束速度領域」と呼ぶ。ここでいう実現不可能なハンド速度ベクトルは、幾何学的な観点から判定される。つまりこの瞬間に、ハンドと対象物の間またはハンドと他のハンドの間の干渉により、ほとんどハンドの変位を生じることなく応力が発生するハンド速度ベクトルを実現不可能なハンド速度ベクトルと定義する。
【０１２８】
このとき、腕アクチュエータ能力や腕部リンク５の関節配置などに関する制約は考えない。つまり、ハンド自身は、対象物がなければ任意の速度で移動可能であることを前提とする。また、ハンドおよび対象物が十分な剛性を備えていれば、剛体とみなす。従って、ある速度ベクトルが拘束速度領域の要素ベクトルならば、そのベクトルの全要素をｋ倍（ｋは任意の正の実数）したベクトルも、拘束速度領域の要素ベクトルとする。尚、便宜上、零ベクトルも拘束速度領域の要素ベクトルとする。
【０１２９】
そして、拘束速度領域の任意のベクトルとその逆ベクトル（大きさが同じで向きが逆のベクトル）からなる集合を改めて「拘束速度空間」と定義する。通常は、拘束速度空間は、全運動速度空間の部分ベクトル空間になっている。
【０１３０】
また、拘束速度領域の任意のベクトルとの内積が零である（即ち、そのベクトルと直交する）全ての全運動速度空間内のベクトルの集合を、改めて「自由速度空間」と呼ぶ。
【０１３１】
ここで、拘束速度空間および自由速度空間内のベクトルを、以下のように定義する。拘束速度空間の要素である方向ベクトルを、改めて「拘束方向ベクトル」と呼び、単に拘束方向というときは、任意の拘束方向ベクトルの向きを指す。また、自由速度空間の要素である方向ベクトルを、改めて「自由方向ベクトル」と呼び、単に自由方向というときは、任意の自由方向ベクトルの向きを指す。
【０１３２】
対象物およびハンドの運動の自由度と拘束に関する定義を、上記の如く拡張する。これにより、複数個（本）の腕を備えるロボットが複数個の腕を使って作業を行う場合でも、全ハンドを拘束方向に動かしてハンドに対象物から反作用（拘束力）を受けさせることで、全ての対象物の運動に影響を与えることなく、ロボットの動バランスを維持したり、傾いた姿勢を復元することができる。
【０１３３】
以下に、複数個の腕による作業の例を示す。先ず、図１２に示すように、対象物が、高さｈ、前方ｂに存在するＺ軸まわりの回転だけができる、半径ｒの円形なホイール１３０であって、このホイール１３０を第１ハンドと第２ハンドの計２個のハンドで把持して回す作業を例として挙げる。
【０１３４】
ある瞬間の第１ハンドの座標が（ｂ，−ｒ，ｈ）であり、第２ハンドの座標が（ｂ，ｒ，ｈ）であるとする。この瞬間の自由速度空間は、任意の実数ωｚ１を持つハンド速度ベクトル（ｒ＊ωｚ１，０，０，０，０，ωｚ１，−ｒ＊ωｚ１，０，０，０，０，ωｚ１）の集合である。これは、対象物の軸まわりに２個のハンドを同じ角度だけ回転させる運動を意味する。
【０１３５】
この瞬間の拘束速度空間は、任意の実数Ｖｘ１，Ｖｙ１，Ｖｚ１，ωｘ１，ωｙ１，ωｚ１，Ｖｘ２，Ｖｙ２，Ｖｚ２，ωｘ２，ωｙ２を持つハンド速度ベクトル（Ｖｘ１，Ｖｙ１，Ｖｚ１，ωｘ１，ωｙ１，ωｚ１，Ｖｘ２，Ｖｙ２，Ｖｚ２，ωｘ２，ωｙ２，−ｒ＊Ｖｘ１−ωｚ１＋ｒ＊Ｖｘ２）の集合である。例えば、ハンド速度ベクトル（Ｖｘ１，０，０，０，０，０，Ｖｘ１，０，０，０，０，０）は、拘束速度空間の速度ベクトルである。これは、ホイール１３０の軸の摩擦が零であるならば、両ハンドをＸ方向に同じ力で押しても、ホイール１３０の回転運動に影響を与えないことを意味する。
【０１３６】
別の例を図１３を参照して説明する。同図に示すように、第１ハンドで固定物（例えば柱）１４０を把持しながら、第２ハンドでヒンジ１４２の付いたドア１４４を開閉する作業を例に挙げる。
【０１３７】
ヒンジ１４２の軸（１４２Ｚ）方向がＺ軸と平行であり、ヒンジ軸１４２ＺのＸ座標がｂ、ヒンジ軸のＹ座標がｒ、この瞬間の第２ハンドの座標を（ｂ，０，ｈ）とする。第１ハンドの座標を（ｂ，−ｃ，ｈ）とすると、自由速度空間は、任意の実数ωｚ２を持つハンド速度ベクトル（０，０，０，０，０，０，ｒ＊ωｚ２，０，０，０，０，ωｚ２）の集合である。
【０１３８】
また、拘束速度空間は、それに直交するベクトル集合なので、任意の実数Ｖｘ１，Ｖｙ１，Ｖｚ１，ωｘ１，ωｙ１，ωｚ１，Ｖｘ２，Ｖｙ２，Ｖｚ２，ωｘ２，ωｙ２を持つ、ハンド速度ベクトル（Ｖｘ１，Ｖｙ１，Ｖｚ１，ωｘ１，ωｙ１，ωｚ１，Ｖｘ２，Ｖｙ２，Ｖｚ２，ωｘ２，ωｙ２，−ｒ＊Ｖｘ２）の集合となる。
【０１３９】
例えば、ハンド速度ベクトル（Ｖｘ１，Ｖｙ１，Ｖｚ１，ωｘ１，ωｙ１，ωｚ１，０，０，０，０，０，０）は、拘束速度空間の速度ベクトルである。これは、第１ハンドで固定物１４０をどのように押しても、ドア１４４の回転運動に影響を与えないことを意味する。
【０１４０】
上記のように、複数のハンドから対象物に対して拘束方向に力を作用させても、対象物にはハンド以外の支持するものからその力を打ち消す拘束力しか発生せず、よって自由方向に摩擦力などを発生しないならば、ハンドおよび対象物の運動は変化しない。
【０１４１】
従って、上記のように定義を拡張することにより、複数のハンドによる作業においても、１個のハンドの場合と同じ原理で、対象物の運動制御に干渉せずに、ロボットの姿勢を安定にすることができる。即ち、複数のハンドによる作業においても、対象物からハンドに作用する力の拘束方向成分を制御することにより、対象物の運動に影響を与えることなく、ロボットの動バランスを維持したり、傾いた姿勢を復元することができる。
【０１４２】
尚、一般的に、ハンドが複数、例えばこの実施の形態のように２個になると、拘束速度空間の次元がハンドが１個の場合よりも高くなるので、この中からロボットの姿勢安定化に効果の高い部分ベクトル空間を選び出し、その要素ベクトルの方向に拘束力を発生させることにより、ハンドが１個の場合よりも高い効果を得ることができる。
【０１４３】
ドアの開閉作業の例でいえば、ドアの開閉制御に干渉しないようにロボットの姿勢を復元させるためには、１個の腕の場合ではドアに上下方向の力を作用させるのが最も効果的であるが、２個の腕の例では、ドアを把持していないハンドで固定物を押すようにすることで、より効果的に動バランスを維持したり、傾いた姿勢を復元することができる。
【０１４４】
以上の原理を前提とし、図５に示す姿勢安定化メイン制御装置１０６の説明に戻る。
【０１４５】
姿勢安定化メイン制御装置１０６は、前記した３つの手法、即ち、
１）実全床反力モーメントを修正する
２）目標慣性力モーメントと目標重力モーメントを修正する
３）実対象物反力の成分の内の拘束方向成分を修正する
の３つの手法を併用するため、図１４に示すように構成される。
【０１４６】
即ち、姿勢安定化メイン制御装置１０６は、姿勢安定化用補償モーメント算出器２００、対象物反力平衡制御装置２０２、対象物反力モーメント偏差分離器２０４、修正目標対象物反力算出器２０６、および各種の分配器（具体的には、対象物反力平衡制御用分配器２０８と補償モーメント分配器２１０）とから構成される。
【０１４７】
姿勢安定化メイン制御装置の各構成要素は全て、制御周期毎（例えば１００ｍｓｅｃ毎）に演算が行われる。ＥＣＵ６２の性能上、同時に複数の演算処理ができない場合には、図１４の矢印上流側から順に演算処理を実行すれば良い。
【０１４８】
以下、姿勢安定化メイン制御装置１０６で処理される変数およびそれらの構成要素について説明（定義）する。
【０１４９】
上述したように、この発明に係る姿勢制御装置は、対象物反力の拘束方向成分を制御することにより、対象物の運動に影響を与えることなく、ロボット１の動バランスを維持したり、傾いた姿勢を復元する制御装置に関する。しかしながら、対象物反力の全ての拘束方向成分が、ロボット１の動バランスを維持したり傾いた姿勢を復元する効果を持っているとは限らない。
【０１５０】
また、ある拘束方向の目標対象物反力が、作業目的や対象物の性質上、変更すべきではない場合もある。つまり、対象物反力の拘束方向成分の内で、ロボット１の動バランスを維持したり、傾いた姿勢を復元するために利用すべき成分を、復元する効果の度合、作業目的および対象物の性質を考慮して適宜選定すべきである。
【０１５１】
そこで、新たに、次の空間と方向を定義する。図２４を参照して説明すると、先ず、全運動速度空間の中で、ロボット１の動バランスを維持したり、傾いた姿勢を復元するために利用すべき全ての拘束方向によって張られるベクトル空間を「操作空間」と呼ぶ。操作空間は、拘束速度空間の部分ベクトル空間となっている。また、操作空間の任意の方向ベクトルを「対象物反力操作方向」あるいは単に「操作方向」と呼ぶ。図示の如く、操作空間は、拘束速度空間から作業空間を除いた、白地の部分に相当する。
【０１５２】
全運動速度空間の方向ベクトルの内、任意の対象物反力操作方向との内積が零である（即ち、その方向と直交する）ものを「対象物反力非操作方向」あるいは単に「非操作方向」と呼び、全ての非操作方向が張る空間を「非操作空間」と呼ぶ。
【０１５３】
即ち、非操作空間は、全運動速度空間の部分ベクトル空間となっており、自由速度空間は、非操作空間の部分ベクトル空間となっている。また、操作空間の任意のベクトルと非操作空間の任意のベクトルは直交する。
【０１５４】
尚、操作方向は、コンプライアンス制御のコンプライアンス定数などと同じく、制御パラメータの一種と考えられる。この値は、一般には一定ではなく、作業が進行するにつれて変化する性質のものである。この値をどのように決定すべきかも重要なことであるが、この発明に係る姿勢制御装置の本質ではないので、この実施の形態では説明の便宜上、操作方向はあらかじめ作業目的、作業パターンおよび対象物の性質に基づいて決定されているものとする（操作方向などは作業中に決定しても良い）。
【０１５５】
以上を前提として姿勢安定化メイン制御装置１０６の上記した構成要素について詳説する。
【０１５６】
姿勢安定化用補償モーメント算出器２００は、ロボット１の姿勢傾斜偏差、より具体的には実上体傾斜と目標上体傾斜の差である上体傾斜偏差を零に収束させるための姿勢安定化用補償モーメント（実全床反力モーメントの修正に使用するモーメント）を算出する装置であり、上記手法１）を達成するための構成である。この装置は、少なくとも、傾斜センサ６０によって検出された上体傾斜角度と目標作業パターン生成器１００が生成した目標上体傾斜角度の偏差に応じ、その偏差を減少するように姿勢安定化用補償モーメントを算出する。
【０１５７】
図１５に、姿勢安定化用補償モーメント算出器２００を最も単純な構成で示す。図示の例では、入出力共に１次元のスカラ量のように示されているが、実際には、Ｘ方向成分とＹ方向成分があり、それぞれに対し、図示の処理が行われる。但し、Ｚ方向成分は、姿勢安定性に関係がないので無視することとする。
【０１５８】
以下、説明すると、先ず実上体傾斜と目標上体傾斜の差である上体傾斜偏差を求める。次に、姿勢安定化制御則部２００ａにおいて上体傾斜偏差の微分値を求め、最後に、式１に従って姿勢安定化用補償モーメントを算出する。

ここで、ｋｐとｋｄは制御ゲインである。尚、これ以外にも、Ｈインフィニティ制御などを用いても良い。
【０１５９】
図１４に戻って説明を続けると、対象物反力平衡制御装置２０２は、検出はできるが予期できない対象物反力を受けたとき、その対象物反力を考慮しながら目標上***置姿勢を修正することにより、目標作業パターン生成器１００で生成された目標慣性力モーメントと目標重力モーメントを修正して動バランスを維持する制御を行なう装置であり、上記手法２）を達成させるための構成である。
【０１６０】
また対象物反力モーメント偏差分離器２０４は、対象物反力を姿勢安定化に利用する拘束方向の成分とそれ以外の成分に分離する装置であり、修正目標対象物反力算出器２０６は対象物反力に姿勢安定化に必要な目標拘束力を加えた目標対象物反力を算出する装置である。
【０１６１】
これら２つの装置２０４，２０６に各種分配器（対象物反力平衡制御用分配器２０８と補償モーメント分配器２１０）を加えたものが、上記手法３）を達成するための構成である。尚、それら装置の詳細は後述する。
【０１６２】
先ず、対象物反力モーメント偏差分離器２０４を説明すると、ハンド４０Ｒ（Ｌ）の力センサ５８で検出される実ハンド力は、ハンド基準点に作用する力と力のモーメントを、ハンド内にローカルに設定された座標系で表現したものとする。ここで、ハンド基準点は、ハンド４０Ｒ（Ｌ）の位置を表わすためのハンド内の基準点である。また、ハンド内にローカルに設定された前記座標系の原点は、ハンド基準点に設定する。
【０１６３】
ところで、ハンド位置姿勢は、ハンド内にローカルに設定された前記座標系を支持脚座標系から見たときの原点位置と座標系の向きを表す。より具体的には、前記原点位置はベクトルで、前記座標系の向きは３行３列マトリックスで表現される。あるいは、ロボット工学で良く用いられる表現である、前記原点位置と前記座標系の向きをまとめて表現する同次行列を用いても良い。
【０１６４】
また、ハンド４０Ｒ（Ｌ）の実際の位置姿勢は、前記した腕メイン制御装置１０４によってアクチュエータ変位指令を介して駆動されることにより、ほぼ最終修正目標ハンド位置姿勢にあると考えられるので、ハンド内にローカルに設定された前記座標系は、最終修正目標ハンド位置姿勢を基準とした座標系であると考えても良い。
【０１６５】
図１６は対象物反力モーメント偏差分離器２０４の構成を示すブロック図である。同図を参照して説明すると、対象物反力モーメント偏差分離器２０４は実対象物反力モーメント成分分離器２０４ａを備え、その分離器２０４ａは支持脚座標系から見た最終修正目標ハンド姿勢を入力し、実ハンド力を支持脚座標系の値に変換する。支持脚座標系に変換された実ハンド力も、ハンド基準点に作用する力と力のモーメントで表現される。尚、支持脚座標系に変換された実ハンド力の作用点を全床反力中心点に変換したものが、実対象物反力である。即ち、これらは表現は異なるが、同一のものを意味している。
【０１６６】
ハンドがｎ個ある場合、支持脚座標系に変換された実ハンド力は、各ハンドの実ハンド力の各成分を順番に並べた１つのベクトルで表現される。即ち、全運動速度空間でのベクトルで表現される。より具体的には、
（Ｆ１ｘ，Ｆ１ｙ，Ｆ１ｚ，Ｍ１ｘ，Ｍ１ｙ，Ｍ１ｚ，Ｆ２ｘ，Ｆ２ｙ，Ｆ２ｚ，Ｍ２ｘ，Ｍ２ｙ，Ｍ２ｚ，・・・，Ｍｎｚ）
の形で表現される。但し、ここで、Ｆｊｘ, Ｆｊｙ, Ｆｊｚは第ｊハンドの基準点に作用する力のｘ，ｙ，ｚ成分、Ｍｊｘ，Ｍｊｙ, Ｍｊｚは第ｊハンドの基準点に作用する力のモーメントのｘ，ｙ，ｚ成分を表わす。
【０１６７】
次に、分離器２０４ａは、支持脚座標系に変換した実ハンド力を、あらかじめ選定された操作方向に従って操作方向成分と非操作方向成分に分離する。尚、ここで注意すべき点は、各成分は、前述したように全運動速度空間のベクトルであって、現実の３次元空間のベクトルではないことである。
【０１６８】
具体的には、分離器２０４ａは、次のような演算によって分離する。先ず、操作空間がｍ次元であったとすると、操作空間の直交基底ベクトルをＡ１，Ａ２，・・・，Ａｍとする。また、非操作空間の直交基底ベクトルをＢ１，Ｂ２，・・・，Ｂｅとする。ここで、ｍとｅの和はハンドの個数の６倍である。
【０１６９】
次いで、支持脚座標系に変換された実ハンド力のベクトルを、Ｆと記述する。また、実ハンド力の操作方向成分をＦａ、実ハンド力の非操作方向成分をＦｂと記述する。これらのベクトルは全運動速度空間のベクトルである。
【０１７０】
実ハンド力の操作方向成分Ｆａと実ハンド力の非操作方向成分Ｆｂは、式２によって求められる。
Ｆａ＝（Ａ１＋Ａ２＋・・・＋Ａｍ）＊Ｆ
Ｆｂ＝（Ｂ１＋Ｂ２＋・・・＋Ｂｅ）＊Ｆ ・・・式２
【０１７１】
次に、分離器２０４ａは、最終修正目標ハンド位置姿勢および実ハンド力の操作方向成分Ｆａに基づき、実ハンド力の操作方向成分Ｆａの全ての成分が、目標全床反力中心点まわりに作用する力の和を求める。これを「目標全床反力中心点まわりの実対象物反力操作方向成分和」と呼ぶ。この力は、３次元空間での力と力のモーメントで表現される。座標系としては支持脚座標系を用いる。また目標全床反力中心点まわりの実対象物反力操作方向成分の内のモーメント成分を「目標全床反力中心点まわりの実対象物反力モーメント操作方向成分和」と呼ぶ。
【０１７２】
次に、分離器２０４ａは、最終修正目標ハンド位置姿勢および実ハンド力の非操作方向成分Ｆｂに基づき、実ハンド力の非操作方向成分Ｆｂの全ての成分が、目標全床反力中心点に作用する力の和を求める。これを「目標全床反力中心点まわりの実対象物反力非操作方向成分和」と呼ぶ。この力も、３次元空間での力と力のモーメントで表現される。座標系としては支持脚座標系を用いる。また目標全床反力中心点まわりの実対象物反力非操作方向成分の内のモーメント成分を「目標全床反力中心点まわりの実対象物反力モーメント非操作方向成分和」と呼ぶ。
【０１７３】
また、対象物反力モーメント偏差分離器２０４は目標ハンド位置作用点変換器２０４ｂを備え、その変換器２０４ｂも、上記のような処理（演算）を目標対象物反力に対しても行う。目標対象物反力も、実ハンド力と同様、全運動速度空間のベクトルである。具体的には先ず、目標対象物反力は目標全床反力中心点を作用点として表現されているので、変換器２０４ｂは、この作用点を一旦、目標ハンド位置（目標ハンドの基準点位置）を作用点とする力と力のモーメントに変換する。これを「目標ハンド位置を作用点とする目標対象物反力」と呼ぶ。
【０１７４】
さらに、対象物反力モーメント偏差分離器２０４は目標対象物反力モーメント成分分離器２０４ｃを備え、分離器２０４ｃは、上記と同様の処理によって操作方向成分と非操作方向成分に分離する。これらをそれぞれ「目標ハンド位置を作用点とする目標対象物反力の操作方向成分」および「目標ハンド位置を作用点とする目標対象物反力の非操作方向成分」と呼ぶ。
【０１７５】
次に、分離器２０４ｃは、目標ハンド位置を作用点とする目標対象物反力の操作方向成分および目標ハンド位置（目標ハンドの基準点位置）に基づき、目標対象物反力の操作方向成分の全ての成分が、目標全床反力中心点に作用する力の和を求める。これを「目標全床反力中心点まわりの目標対象物反力操作方向成分和」と呼ぶ。この力は、３次元空間での力と力のモーメントで表現される。座標系としては支持脚座標系を用いる。また、目標全床反力中心点まわりの目標対象物反力操作方向成分の内のモーメント成分を「目標全床反力中心点まわりの目標対象物反力モーメント操作方向成分和」と呼ぶ。
【０１７６】
次に、分離器２０４ｃは、目標ハンド位置を作用点とする目標対象物反力の非操作方向成分および目標ハンド位置（目標ハンドの基準点位置）に基づき、目標対象物反力の非操作方向成分の全ての成分が、目標全床反力中心点に作用する力の和を求める。これを「目標全床反力中心点まわりの目標対象物反力非操作方向成分和」と呼ぶ。この力も、３次元空間での力と力のモーメントで表現される。座標系としては支持脚座標系を用いる。また、目標全床反力中心点まわりの目標対象物反力非操作方向成分の内のモーメント成分を「目標全床反力中心点まわりの目標対象物反力モーメント非操作方向成分和」と呼ぶ。
【０１７７】
次いで、加算点２０４ｄにおいて、かくして得られた目標全床反力中心点まわりの実対象物反力モーメント操作方向成分和から目標全床反力中心点まわりの目標対象物反力モーメント操作方向成分和を減じることにより、目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和を得る。
【０１７８】
また、加算点２０４ｅにおいて、目標全床反力中心点まわりの実対象物反力モーメント非操作方向成分和から目標全床反力中心点まわりの目標対象物反力モーメント非操作方向成分和を減じることにより、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和を得る。
【０１７９】
尚、目標全床反力中心点まわりの対象物反力偏差の内のモーメント成分を除く成分は、ロボット１の姿勢にあまり関与しないので、この実施の形態では無視することとした。また、最終修正目標ハンド位置姿勢を用いる代り、脚部リンク２および腕部リンク５の実関節角を用いてキネマティクス演算によって算出される実ハンド位置姿勢を用いても良い。
【０１８０】
図１４の説明に戻ると、上記のようにして得た目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和は、後に述べるように補償全床反力モーメントの算出に使用される。他方、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和は、対象物反力平衡制御用分配器２０８に入力される。
【０１８１】
この対象物反力平衡制御用分配器２０８を図１７を参照して説明すると、分配器２０８は関数２０８ａ，２０８ｂを備え、対象物反力平衡制御用分配器２０８は、それら関数２０８ａ，２０８ｂに目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和を入力し、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和と、対象物反力平衡制御以外で平衡をとるべき対象物反力偏差モーメント和を出力させる。
【０１８２】
尚、対象物反力平衡制御とは、前述の手法２）を達成するために対象物反力平衡制御装置２０２で行なわれる制御であり、概説すると、前記したように、上***置をずらすことによって重力モーメントを発生させ、よって対象物反力偏差モーメント和を打ち消す制御である。
【０１８３】
また、図１７に示す例では、入出力共に１次元のスカラ量のように示されているが、実際には、Ｘ方向成分とＹ方向成分があり、それぞれに対して図示の処理が行われる。但し、Ｚ方向成分は、姿勢安定性に関係がないので無視することとする。
【０１８４】
図示の如く、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和を出力する関数２０８ａとしては、上下限リミッタ特性（飽和特性）を持った関数を用いると共に、対象物反力平衡制御以外で平衡をとるべき対象物反力偏差モーメント和を出力する関数２０８ｂとしては不感帯特性を持った関数を用いる。
【０１８５】
このような関数特性を持たせた場合、動バランスを維持するために、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和の絶対値がある設定値以下のとき、そのモーメントを対象物反力平衡制御によって打ち消し、そのモーメントが設定値を越えたとき、超えた分を主にハンドの拘束方向に対象物反力を発生させて打ち消すように制御系が働く。即ち、前記上下限リミッタ特性を持たすことにより、上***置のずらし量に制限を持たせ、幾何学的に無理な姿勢になるのを防止することができる。
【０１８６】
尚、入出力関係を決定する２つの関数２０８ａ，２０８ｂには、図示例以外の関数を用いても良い。例えば、図１８に２０８ｃ，２０８ｄで示すような曲線でも良い。図１７および図１８のいずれにしても、ロボット１の特性、対象物の特性および作業内容に応じ、関数を選定すべきである。
【０１８７】
また、入出力関係を決定する２つの関数の出力和は、基本的には入力に一致させるべきであるが、厳密に一致させる必要はない。特に、脚コンプライアンス制御や腕コンプライアンス制御は、目標値の絶対値が大きいと、発生する力が目標に対して小さめになりやすい傾向があるので、これを補正する意味で、入力の絶対値が大きい領域では、関数出力の和の絶対値を入力の絶対値より大きくした方が良い場合もある。また、Ｘ方向成分とＹ方向成分を独立して分配するのでなく、Ｘ方向成分とＹ方向成分の両方を入力とした２入力関数を用いて、各出力を決定しても良い。
【０１８８】
図１４の説明に戻ると、対象物反力平衡制御用分配器２０８でかくして分配された、対象物反力平衡制御以外で平衡をとるべき対象物反力偏差モーメント和は、さらに乗算点２１４で−１倍されて極性が反転させられ、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントとして補償モーメント分配器２１０に入力される。また、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和は、対象物反力平衡制御装置２０２に入力される。
【０１８９】
次いで、先に概説した対象物反力平衡制御装置２０２について詳細に説明する。
【０１９０】
先ずその機能について概説すると、対象物反力平衡制御装置２０２は、上述したように目標上***置姿勢などを入力し、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和に動力学的に平衡するように目標上***置姿勢と目標全床反力を修正し、修正目標上***置姿勢と対象物反力平衡制御用補償全床反力モーメントを出力する。
【０１９１】
即ち、装置２０２は、長期的には、目標上***置姿勢をずらすことによって発生する重力モーメントが、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和に動力学的に平衡し、目標全床反力の修正量が零に戻る特徴を持つ。
【０１９２】
前述の如く、対象物反力平衡制御装置２０２は上記手法２）を解決するための構成であり、本出願人が既に特開平１０−２３０４８５号公報で提案している技術であるが、この実施の形態においては、かかる公報に記載される対象物反力平衡制御装置の後半部の構成のみが使用されると共に、「対象物反力偏差」が入力されていた部分に、前記した「対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和」が入力されるように変更された点で異なる。
【０１９３】
図１９は対象物反力平衡制御装置２０２の構成を示すブロック図であり、最終到達目標重心位置摂動量算出部２０２ａ、モデル制御則演算器２０２ｂ、および摂動動力学モデル２０２ｃを備える。
【０１９４】
これらへの入力は、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和、目標上***置姿勢、最終修正目標ハンド位置姿勢、最終修正目標上***置姿勢、最終修正目標足部位置姿勢である。尚、この内、最終修正目標ハンド位置姿勢、最終修正目標上***置姿勢および最終修正目標足部位置姿勢は、対象物反力平衡制御において近似演算を用いる場合、即ち、摂動動力学モデル２０２ｃが近似モデルである場合には不要である。
【０１９５】
尚、前述の姿勢安定化用補償モーメントは、部分的にも対象物反力平衡制御装置２０２には入力されない。姿勢安定化用補償モーメントは、姿勢が傾いた結果ずれた重心位置を復元させるためにロボットに外部から与えられるべきモーメントであるが、対象物反力平衡制御装置２０２の機能は、目標姿勢の重心をずらすことによって定常的な対象物反力偏差を打ち消すことであり、姿勢が傾いた結果ずれた重心を復元することはできないためである。
【０１９６】
以下詳説すると、入力された対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和は、最終到達目標重心位置摂動量算出部２０２ａに入力される。
【０１９７】
この対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和を長期的に打ち消してバランスをとるための重心摂動量を「最終到達目標重心位置摂動量」と呼ぶ。最終到達目標重心位置摂動量算出部２０２ａは、上記和から最終到達目標重心位置摂動量を算出して出力する。
【０１９８】
最終到達目標重心位置摂動量は、モデル制御則演算器２０２ｂに入力される。モデル制御則演算器２０２ｂについて説明すると、最終到達目標重心位置摂動量と、摂動動力学モデル２０２ｃが出力する目標重心位置摂動量との差を重心変位偏差とすると、モデル制御則演算器２０２ｂは、この重心変位偏差を零に収束させるためのモーメントである対象物反力平衡制御用補償全床反力モーメントをＰＤ制御則によって決定して出力する。
【０１９９】
尚、摂動動力学モデル２０２ｃは、目標作業パターンの運動（摂動）に、ある拘束条件を与えておいた場合の、モデルのための目標全床反力モーメント摂動量と上***置姿勢摂動量との関係を表すモデルである。
【０２００】
モデル制御則演算器２０２ｂの出力直後の加算点２０２ｄについて説明すると、加算点２０２ｄによって、摂動動力学モデルには、対象物反力平衡制御用補償全床反力モーメントと、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和が加算されたものが、モデルのための目標全床反力モーメント摂動量（モデル入力量）として入力され、その入力に対応するように前記摂動動力学モデルによって上***置姿勢摂動量が算出される。それが加算点２０２ｄで目標上***置姿勢に加算され、修正目標上***置姿勢が出力される。
【０２０１】
図１４の説明に戻ると、このように、対象物反力平衡制御装置２０２の出力は、入力した目標上***置姿勢を修正した修正目標上***置姿勢と、対象物反力平衡制御用補償全床反力モーメントである。出力された対象物反力平衡制御用補償全床反力モーメントは、同図に示すように、続いて述べる補償モーメント分配器２１０に入力される。
【０２０２】
尚、上記で、装置２０２から対象物反力平衡制御用補償全床反力の内のモーメント成分のみが出力されるようにしたのは、対象物反力平衡制御用補償全床反力の内、ロボットの姿勢安定化のための特に重要な成分は、Ｘ軸まわりモーメント成分とＹ軸まわりモーメント成分であるためである。モーメント成分以外も考慮した制御系にした方がより良いが、顕著な効果は期待できない。
【０２０３】
次いで、補償モーメント分配器２１０について説明する。補償モーメント分配器２１０は、図２０に示すように、対象物反力平衡制御用補償全床反力モーメント、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントおよび姿勢安定化用補償モーメントを入力し、それらを目標拘束力モーメントと補償全床反力主モーメントに分配する、２つの関数を備える。
【０２０４】
ここで、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントは、前記したように、対象物反力平衡制御以外の手段で平衡をとるべき対象物反力偏差モーメント和を乗算点２１４で−１倍した値である。
【０２０５】
また、目標拘束力モーメントは、前記入力の内で、拘束方向の対象物反力によって目標全床反力中心点まわりに発生させるべきモーメントであり、補償全床反力主モーメントは、前記入力の内で、目標全床反力中心点まわりに全床反力によって発生させるべきモーメントである。
【０２０６】
以下、図２０を参照して分配処理について説明する。尚、図示の例では入出力共に１次元のスカラ量のように示されているが、実際にはＸ方向成分とＹ方向成分があり、それぞれに対して処理が行われる。但し、Ｚ方向成分は姿勢安定性に関係がないので無視することとする。
【０２０７】
分配器２１０は加算点２１０ａを備え、加算点２１０ａで対象物反力平衡制御用補償全床反力モーメントと、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントの和を求めて２つの関数に入力する。ここで、姿勢安定化用補償モーメントをｕ、前記和をｖと置き、前記した２つの関数の中の１つである関数ｇ１（ｕ，ｖ）の値を求め、これを補償全床反力主モーメントとする。また、他方の関数ｇ２（ｕ，ｖ）の値を求め、これを目標拘束力モーメントとする。但し、それぞれの関数は２入力１出力の関数である。
【０２０８】
この関数は、例えば以下に示す式のようにすれば良い。尚、式中のＣｍａｘおよびＣｍｉｎは設定値である。
ｕ＋ｖ＞Ｃｍａｘのとき
ｇ１（ｕ，ｖ）＝Ｃｍａｘ
ｕ＋ｖ＜Ｃｍｉｎのとき
ｇ１（ｕ，ｖ）＝Ｃｍｉｎ
Ｃｍｉｎ≦ｕ＋ｖ≦Ｃｍａｘのとき
ｇ１（ｕ，ｖ）＝ｕ＋ｖ ・・・式３
ｇ２（ｕ，ｖ）＝ｕ＋ｖ−ｇ１（ｕ，ｖ） ・・・式４
【０２０９】
図２０では、式３、式４で表わされる関数を、入力ｕと入力ｖを水平面上の直交軸とし、出力値を高さで表している。
【０２１０】
基本的には、補償モーメント分配器の入力和（対象物反力平衡制御用補償全床反力モーメントと対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントと、姿勢安定化用補償モーメントの和）が、出力和（目標拘束力モーメントと補償全床反力主モーメントの和）に一致するように、２つの関数を選定すべきである。即ち、ｇ１（ｕ，ｖ）＋ｇ２（ｕ，ｖ）＝ｕ＋ｖが恒等的に成立するように、２つの関数を選定すべきである。
【０２１１】
尚、対象物反力平衡制御用補償全床反力モーメントをｕ、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントをｖ、姿勢安定化用補償モーメントをｗと置き、補償全床反力主モーメントと目標拘束力モーメントを、それぞれ３入力の関数ｇ１（ｕ，ｖ，ｗ）、関数ｇ２（ｕ，ｖ，ｗ）の値によって決定しても良い。
【０２１２】
その場合でも、基本的には、ｇ１（ｕ，ｖ，ｗ）＋ｇ２（ｕ，ｖ，ｗ）＝ｕ＋ｖ＋ｗが恒等的に成立するように関数を選定すべきである。しかしながら、複合コンプライアンス制御などの制御偏差を考慮し、出力和を大きめにしても良い。特に、姿勢安定化用補償モーメントはフィードバック量であるから、姿勢安定化用補償モーメントに影響を受けた出力成分の和が、姿勢安定化用補償モーメントと数十パーセント違っていてもあまり問題にならない。
【０２１３】
図１４の説明に戻ると、補償モーメント分配器２１０から出力される、一方のモーメントである目標拘束力モーメントは、加算点２１５に送られ、そこで前述の目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和から減算される。
【０２１４】
また、補償モーメント分配器２１０から出力される、他方のモーメントである補償全床反力主モーメントから加算点２１６で、前述の目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和と目標拘束力モーメントの差が減算され、補償全床反力モーメントが出力される。
【０２１５】
但し、腕アクチュエータ９２のコンプライアンス制御の能力が高く、目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和がほぼ目標拘束力モーメントに一致するように制御される場合、加算点２１６で目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和と目標拘束力モーメントの差を減じることなく、補償全床反力主モーメントをそのまま補償全床反力モーメントとしても良い。
【０２１６】
尚、加算点２１６を削除し、補償全床反力モーメントを補償全床反力主モーメントとし、その代わりに、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和から、目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和と目標拘束力モーメントの差（加算点２１５の出力）を減じるようにしても良い。
【０２１７】
前記した目標拘束力モーメントは、修正目標対象物反力算出器２０６にも入力される。
【０２１８】
図２１に、修正目標対象物反力算出器２０６の構成を示す。同図に示すように、修正目標対象物反力算出器２０６は、目標ハンド位置姿勢（あるいは最終修正目標ハンド位置姿勢）、目標拘束力モーメント、目標全床反力中心点位置、操作方向および目標対象物反力を入力し、それら入力に基づいて修正目標対象物反力を算出して出力する。
【０２１９】
以下、その処理について説明すると、算出器２０６は目標ハンド力修正量決定器２０６ａ、変換器２０６ｂおよび加算点２０６ｃを備える。目標ハンド力修正量決定器２０６ａは、目標ハンド位置（あるいは最終修正目標ハンド位置）を作用点とするように表現された目標対象物反力の修正量である目標ハンド力修正量を以下のように決定する。
【０２２０】
即ち、決定器２０６ａは、目標ハンド力修正量が目標ハンド位置（あるいは最終修正目標ハンド位置）に作用したときに、目標全床反力中心点に作用するモーメントの和と目標拘束力モーメントの差が零に近づくように、目標ハンド力修正量を決定する。但し、決定器２０６ａは、目標ハンド力修正量が、操作方向の力と力のモーメントであって、非操作方向成分を含まないように決定する。尚、目標ハンド力修正量は、支持脚座標系で表現される。
【０２２１】
最も簡単な決定法を以下に示す。先ず、操作方向の内のある２つのベクトルを自由に選択する。各ベクトルは、操作空間の直交基底ベクトルＡ１，Ａ２，・・・，Ａｍを適当に線形結合させることによって得られる。このベクトルをそれぞれＱ１，Ｑ２とする。
【０２２２】
次いで目標ハンド力修正量を、ａＱ１＋ｂＱ２の形とする。ここで、ａ，ｂは係数である。
【０２２３】
目標ハンド力修正量ａＱ１＋ｂＱ２が目標ハンド位置（あるいは最終修正目標ハンド位置）に作用したとき、目標全床反力中心点に作用する合力のモーメントＭを、力学演算により求める。
【０２２４】
求められたＭのＸ成分をＭｘ、Ｙ成分をＭｙとすると、Ｍｘ，Ｍｙとａ，ｂの関係は、次の連立１次方程式の形になる。ここで、ｋ１１，ｋ１２，ｋ２１およびｋ２２は係数である。
Ｍｘ =ａｋ１１＋ｂｋ１２
Ｍｙ =ａｋ２１＋ｂｋ２２ ・・・式５
【０２２５】
次に、Ｍｘが目標拘束力モーメントのＸ成分、Ｍｙが目標拘束力モーメントのＹ成分に一致するように、ａおよびｂを決定する。具体的には、式５の連立方程式をａおよびｂに関して解き、Ｍｘに目標拘束力モーメントのＸ成分の値、Ｍｙに目標拘束力モーメントのＹ成分の値を代入することにより、ａおよびｂを得る。
【０２２６】
次に、前記ベクトルＱ１，Ｑ２および前記ａ，ｂを用いて目標ハンド力修正量ａＱ１＋ｂＱ２の値を得る。得られた修正量は変換器２０６ｂに入力され、変換器２０６ｂは、目標ハンド力修正量を、作用点を目標ハンド位置（あるいは最終修正目標ハンド位置）から全床反力中心点に変更した表現に変換することにより、全床反力中心点を作用点とする目標対象物反力修正量を出力する。
【０２２７】
最後に、加算点２０６ｃで、全床反力中心点を作用点とする目標対象物反力に、全床反力中心点を作用点とする目標対象物反力修正量を加えることにより、全床反力中心点を作用点とする修正目標対象物反力が出力される。尚、修正目標対象物反力は、全運動速度空間でのベクトルである。
【０２２８】
図１４の説明に戻ると、以上のようにして得た修正目標対象物反力および前記した修正目標上***置姿勢ならびに補償全床反力モーメントが、姿勢安定化メイン制御装置１０６の最終的な出力となる。そして、図５に示すように、修正目標対象物反力が腕メイン制御装置１０４に、補償全床反力モーメントが脚メイン制御装置１０２に、修正目標上***置姿勢がその両方に入力される。
【０２２９】
脚メイン制御装置１０２と腕メイン制御装置１０４の機能を再説すると、脚メイン制御装置１０２では、前述のように、目標全床反力中心点位置に作用する実全床反力のモーメント成分が、目標全床反力モーメント（通常は零）と補償全床反力モーメントの和に一致するように目標足部位置姿勢を修正し、さらに、修正目標上***置姿勢と修正された目標足部位置姿勢から決定される目標脚関節変位に実関節変位が追従するように、アクチュエータ変位指令を出力して脚関節アクチュエータを制御する。
【０２３０】
また、腕メイン制御装置１０４では、前述のように、ハンド４０Ｒ（Ｌ）の力センサ５８によって検出される実対象物反力（実ハンド力）と修正目標対象物反力の差に応じ、差が零に近づくように、ＰＤ制御などによって目標ハンド位置姿勢を修正する。さらに、修正目標上***置姿勢（あるいは最終修正目標上***置姿勢）と修正された目標ハンド位置姿勢から決定される目標腕関節変位に実関節変位が追従するように、アクチュエータ変位指令を出力して腕関節アクチュエータを制御する。
【０２３１】
以上のように、姿勢安定化メイン制御装置１０６は、目標全床反力中心点に作用する対象物反力偏差モーメントを、操作方向成分と非操作方向成分とに分離し、前記非操作方向成分によって発生する動力学的アンバランスを、その一部を対象物反力平衡制御装置によって打ち消し、残りを、目標全床反力中心点に作用すべき目標対象物反力モーメント操作方向成分を修正することにより、言い換えれば、腕の動作を修正して前記操作方向成分を変化させることにより打ち消すように構成した。
【０２３２】
換言すれば、先の提案技術において、対象物から受ける予期せぬ反力が急激に変化したとき、対象物反力平衡制御装置２０２によって行なわれていた、足部２２Ｒ（Ｌ）（および脚部リンク２）を動かして踏み止めさせる（踏ん張らせる）動作を、腕部リンク５の動作によって軽減、あるいはなくすことができる。このため、全床反力モーメントが限界を越えて発生する恐れがなく、よってロボット１の動バランスを維持することができ、ロボットが傾いたり、転倒したりすることを防止できる。
【０２３３】
また、操作方向成分が変化するように腕の動作を修正することから、ロボット１の動バランスを維持したり、傾いた姿勢を復元しても、対象物の運動に影響を与えることがない。
【０２３４】
最後に、上記した制御系の安定性について説明する。
【０２３５】
目標全床反力中心点に作用すべき目標対象物反力モーメント操作方向成分の修正量である目標拘束力モーメントと、目標全床反力中心点まわりの対象物反力偏差モーメントの操作方向成分和と非操作方向成分和の関係は、前記した図１４に示すようになる。
【０２３６】
理解の便宜のため、姿勢安定化メイン制御装置１０６において対象物反力モーメント偏差分離器２０４が分離する、目標全床反力中心点に作用する対象物反力偏差モーメントの操作方向成分和と非操作方向成分和の内、非操作方向成分和によって発生する動力学的アンバランスを全て目標全床反力中心点に作用すべき目標対象物反力モーメント操作方向成分を修正することによって打ち消すように構成したと仮定する。即ち、対象物反力平衡制御を働かせない場合を想定する。
【０２３７】
その場合、腕制御系（図５に示す腕メイン制御装置１０４やアクチュエータ９２、アクチュエータ駆動装置８８などからなる制御系）と姿勢安定化メイン制御装置１０６からなる制御系は、対象物反力に注目すると、図２２のように簡略化することができる。尚、図２２における外乱モーメントは、対象物に想定していた性質と異なる性質を持っていたため、対象物が予期せぬ挙動を示したことによって発生する対象物反力、即ち、想定外の対象物反力である。例えば、対象物に発生する摩擦が、想定していたものと異なっていたことによって発生する。外乱モーメントの中の操作方向成分を「外乱モーメント操作方向成分」、非操作方向成分を「外乱モーメント非操作方向成分」と呼ぶ。
【０２３８】
図２２で加算点２２０で入力される外乱モーメント非操作方向成分が変化すると、これを打ち消すように目標拘束力モーメントが決定され、加算点２２２で加算されることで、目標全床反力中心点まわりの目標対象物反力モーメント操作方向成分和が修正され、操作方向腕コンプライアンス制御部で目標ハンド位置姿勢が修正量だけ修正され、さらに腕部リンク５のアクチュエータ変位指令が修正され、ロボット１の腕部リンク５が駆動されて動バランスが保持される。また、外乱モーメント操作方向成分が変化しても、腕のコンプライアンス制御への目標値は変化しない。
【０２３９】
他方、姿勢安定化メイン制御装置１０６が、前記非操作方向成分和によって発生する動力学的アンバランスを、目標全床反力中心点に作用すべき目標対象物反力モーメント非操作方向成分和を修正することによって打ち消すように構成したと仮定すると、腕制御系と姿勢安定化メイン制御装置１０６からなる制御系は、対象物反力に注目すると、図２３のように簡略化することができる。
【０２４０】
この場合、姿勢安定化メイン制御装置を含むフィードバックループは正帰還となり、発散する。
【０２４１】
即ち、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和から姿勢安定化メイン制御装置によって目標拘束力モーメントが算出され、それが加算点２２６で入力（目標全床反力中心点まわりの目標対象物反力モーメント非操作方向成分和）に加算されて目標全床反力中心点まわりの修正目標対象物反力モーメント非操作方向成分和が算出され、それが加算点２２８で目標全床反力中心点まわりの実対象物反力モーメント非操作方向成分和から減算され、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和が算出されるまでのフィードバックループは、正帰還となり、発散する。
【０２４２】
即ち、図２３に示す構成においては、ひとたび外乱モーメント非操作方向成分が発生すると、これを打ち消そうと目標全床反力中心点まわりの修正目標対象物反力モーメント非操作方向成分和が限りなく増加あるいは減少して発散することとなる。
【０２４３】
それに対し、この実施の形態に係る移動ロボットの姿勢制御装置にあっては、図１４などに示す如く、目標全床反力中心点まわりの対象物反力モーメントを操作方向成分和と非操作方向成分和に分離し、かつ、図２２に示す如く、正帰還にならないようにフィードバックループを構成することにより、このような発散を防ぐようにした。即ち、非操作方向側でフィードバックされた値が操作方向側に加算されるように構成することで、そのような発散を防ぐようにした。
【０２４４】
図２５は、この発明の第２の実施の形態に係る移動ロボットの姿勢制御装置である。
【０２４５】
第１の実施の形態と相違する点に焦点をおいて説明すると、第２の実施の形態においては、対象物反力平衡制御用分配器２０８の出力の中、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和を零（同図に破線で示す）とすると共に、分配器２０８の出力を全てそれ以外の手段で平衡をとるべき対象物反力偏差モーメント和とした。
【０２４６】
尚、残余の構成および効果は第１の実施の形態のそれと異ならない。第２の実施の形態に係る移動ロボットの姿勢制御装置は上記の如く構成したので、第１の実施の形態に係る装置と同様の効果を得ることができる。
【０２４７】
図２６は、この発明の第３の実施の形態に係る移動ロボットの姿勢制御装置である。
【０２４８】
第１の実施の形態と相違する点に焦点をおいて説明すると、第３の実施の形態においては、２足の脚式移動ロボット１に代え、図示のような車輪式の移動ロボットについて姿勢制御を行うようにした。
【０２４９】
以下、説明すると、図示の車輪式の移動ロボット３００は、円筒形の基体（上体）３０２と、それに上下（Ｚ軸方向）にストロークするアクティブサスペンション（図示せず）を介して取りつけられる４個の車輪（図で２個のみ示す）３０４を備える。基体３０２の上部にはマウント３０６が回転自在に取りつけられると共に、その上に１個の屈曲自在なアーム（腕部リンク）３１０が取りつけられる。
【０２５０】
即ち、アーム３１０は、マウント３０６に取りつけられる第１リンク３１０ａと、第１リンク３１０ａに関節（図示せず）を介して取りつけられる第２リンク３１０ｂと、第２リンク３１０ｂの自由端側に関節（図示せず）を介して取りつけられるハンド３１０ｃからなる。関節は、それぞれ電動モータなどのアクチュエータを内蔵する。
【０２５１】
基体３０２の内部には、マイクロコンピュータからなる電子制御ユニット（ＥＣＵ）３１２が格納される。基体３０２の重心位置の付近には傾斜センサ（図示せず）が配置され、Ｚ軸に対する基体３０２の傾きとその角速度に応じた出力を生じる。車輪３０４のそれぞれには加重センサ（図示せず）が配置され、接地面から４個の車輪３０４にそれぞれ作用する床反力（加重）Ｆ１，Ｆ２，．．を検出する。
【０２５２】
ここで、マウント３０６のＺ軸まわりの回転角度をθ１、第１リンク３１０ａのＺ軸に対する角度をθ２、第１リンク３１０ａに対する第２リンク３１０ｂの相対角度をθ３、第２リンク３１０ｂに対するハンド３１０ｃの相対角度をθ４とすると、移動ロボット３００の目標動作は、θ１からθ４の動作パターンとＺＭＰで表すことができる。
【０２５３】
また、図示の移動ロボット３００の姿勢制御において、第１リンク３１０ａのＺ軸に対する角度θ２を制御するものとすると、θ２が第１の実施の形態の脚式移動ロボット１の目標上***置に、θ１，θ３，θ４の組が目標上体姿勢に相当する。また、θ４がハンド３１０ｃの位置姿勢に相当する。アクティブサスペンションを制御する電子制御ユニット（ＥＣＵ）３１２の動作が第１の実施の形態の図８に示す構成の中の複合コンプライアンス動作決定部の動作に相当し、加重ＦをＦ１からＦ４に分配して制御する。
【０２５４】
従って、第３の実施の形態において車輪式の移動ロボット３００について上記のような制御を行うとき、第１の実施の形態で述べた脚式移動ロボット１の姿勢制御と同様の効果を得ることができる。
【０２５５】
以上のように、第１から第３の実施の形態に係る移動ロボットの姿勢制御装置にあっては、少なくとも基体（上体３）と、移動機構（脚部リンク２あるいは車輪３０４）と、対象物に力を作用することができる、少なくとも１個の腕部リンク５（あるいはアーム３１０）からなる移動ロボット１，３００の姿勢制御装置において、前記腕部リンクに実際に作用する対象物からの反力のうちの設定された所定方向の成分である第１の外力を検出あるいは推定する所定方向成分検出手段（検出については力センサ５８，荷重センサ、ＥＣＵ６２，３１２、推定についてはオブザーバ）を備えると共に、前記所定方向成分検出手段が検出あるいは推定した第１の外力（目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和）に応じ、前記所定方向に直交する方向に前記腕部リンクに第２の外力（対象物反力平衡制御以外の手段に要求される対象物反力補償モーメント）が作用するように、換言すれば、前記目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和に基づいて得られる対象物反力平衡制御以外の手段で平衡をとるべき対象物反力偏差モーメント和の極性を反転して得られる、即ち、それに直交する対象物反力平衡制御以外の手段に要求される対象物反力補償モーメント和の極性を反転して得られる、即ち、それに直交する対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントが作用するように、前記腕部リンクを駆動して、換言すれば目標値を修正することを必要とせず、前記ロボットの姿勢を安定させるように制御する如く構成した。
【０２５６】
また、少なくとも基体（上体３）と、移動機構（脚部リンク２あるいは車輪３０４）と、対象物に力を作用することができる、少なくとも１個の腕部リンク５（あるいはアーム３１０）からなる移動ロボット１，３００の姿勢制御装置において、少なくとも前記ロボットの目標腕部リンク位置姿勢（目標ハンド位置姿勢）と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作（目標対象物反力あるいは目標位置姿勢）を生成する目標動作生成手段（目標作業パターン生成器１００）、前記対象物反力の実際値である実対象物反力（実ハンド力）を検出あるいは（オブザーバを介して）推定する実対象物反力検出手段（検出については力センサ５８，荷重センサ、ＥＣＵ６２，３１２、推定についてはオブザーバ）、少なくとも前記検出あるいは推定された実対象物反力に基づき、前記ロボットの姿勢が安定するように前記目標動作のうちの少なくとも前記目標腕部リンク位置姿勢または前記目標対象物反力を修正する姿勢安定化制御手段（姿勢安定化メイン制御装置１０６）、および少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置（アクチュエータ９０，９２）を備える如く構成した。
【０２５７】
また、前記姿勢安定化制御手段は、少なくとも前記実対象物反力と前記目標対象物反力との差を示す対象物反力偏差（目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和）に基づき、前記ロボットの姿勢が安定するように前記目標動作を修正する如く構成した。
【０２５８】
また、前記姿勢安定化制御手段は、前記対象物反力偏差の第１の成分（目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和）によって発生する転倒力の全部あるいは一部を、前記第１の成分から得られる第２の成分（対象物反力平衡制御以外の手段に要求される対象物反力補償モーメント）によって打ち消すように、前記目標動作を修正する如く構成した。
【０２５９】
また、少なくとも基体（上体３）と、移動機構（脚部リンク２あるいは車輪３０４）と、対象物に力を作用することができる、少なくとも１個の腕部リンク５（あるいはアーム３１０）からなる移動ロボット１，３００の姿勢制御装置において、少なくとも前記ロボットの目標腕部リンク位置姿勢（目標ハンド位置姿勢）と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作（目標対象物反力あるいは目標位置姿勢）を生成する目標動作生成手段（目標作業パターン生成器１００）、前記ロボットの姿勢傾き偏差（上体傾斜角度）を検出し、前記検出した姿勢傾き偏差、より具体的には検出値に基づいて算出される姿勢安定化用補償モーメント）が零に近づくように前記目標動作のうちの少なくとも前記目標腕部リンク位置姿勢または前記目標対象物反力を修正する姿勢安定化制御手段（姿勢安定化メイン制御装置１０６）、および少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置（アクチュエータ９０，９２）を備える如く構成した。
【０２６０】
尚、上記において、目標全床反力中心点まわりの実対象物反力非操作方向成分和は、実対象物反力の全ての成分が、目標全床反力中心点まわりに作用する力の和から、目標全床反力中心点まわりの実対象物反力操作方向成分和を減じることによって求めても良い。なぜなら、目標全床反力中心点まわりの実対象物反力操作方向成分和と目標全床反力中心点まわりの実対象物反力非操作方向成分和の和は、実対象物反力の全ての成分が、目標全床反力中心点まわりに作用する力の和に等しいからである。
【０２６１】
逆に、目標全床反力中心点まわりの実対象物反力操作方向成分和は、実対象物反力の全ての成分が、目標全床反力中心点まわりに作用する力の和から、目標全床反力中心点まわりの実対象物反力非操作方向成分和を減じることによって求めても良い。
【０２６２】
さらには、目標全床反力中心点まわりの対象物反力操作方向成分和と、目標全床反力中心点まわりの対象物反力非操作方向成分和に関しても同様の方法で求めても良い。
【０２６３】
また、姿勢安定化メイン制御装置１０６において、目標対象物反力を修正する代わりに目標ハンド位置姿勢を変更しても良い。より詳細に説明すると、腕メイン制御装置１０４は、通常、対象物反力とハンド位置姿勢を目標値に一致させるように制御することはできないので、対象物反力の制御とハンド位置姿勢の制御にトレードオフが働き、目標ハンド位置姿勢を変更するだけでも実対象物反力が変化する。
【０２６４】
従って、このときの目標ハンド位置姿勢の変更量と実対象物反力の変化の比（即ち腕メイン制御系のコンプライアンス定数）が分かっていれば、上述の実施の形態で求められた目標対象物反力の修正量に、その比を乗じることにより、目標ハンド位置姿勢の修正量を求めることができるためである。
【０２６５】
また、脚式移動ロボットとして２個の脚部リンクと２個の腕部リンクを備えたヒューマノイドロボットを例にとって説明したが、２個以外の個数の脚部リンクを備えた脚式移動ロボットロボットでも良く、腕部リンクの個数も１個あるいは３個以上でも良い。
【０２６６】
また、脚部リンクであっても、床以外の対象物に作用するものであれば、腕部リンクとみなして良い。例えば、昆虫型の６個の脚部リンクを備えたロボットにおいて、頭部側の前の２個の脚部リンクを用いて物を持ち上げる場合、その脚部リンクを腕部リンクとみなすことができる。
【０２６７】
また、ロボットの動力学的アンバランスをＸ軸方向とＹ軸方向に対する傾きとして説明したが、遊脚の振りなどに起因するＺ軸まわりのスピンに対しても同様に適用することができる。
【０２６８】
また、上体リンクの曲げやひねりのためのアクチュエータを設けなかったが、それを追加するとき、上体アクチュエータ制御装置も必要となる。但し、上体リンクの曲げやひねりは、腕または脚の付け根側に関節を追加したことと等価であるので、概念上、腕または脚のアクチュエータとみなすことができる。即ち、上体アクチュエータ制御装置は、腕または脚の制御装置の一部として含まれると考えることができる。
【０２６９】
また、腕部リンクの制御に、仮想コンプライアンス制御装置以外の別の手段、例えば、電動アクチュエータを電流指令型のアンプによって制御する手段を用いて関節トルクを制御し、その結果、間接的に対象物反力を制御するようにしても良い。その制御にはハンドに力センサを配置する必要はないが、対象物反力制御装置用にやはり力センサを設けるのが望ましい。
【０２７０】
また、ハンド４０Ｒ（Ｌ）の力センサ５８の代わりに、関節トルクから実対象物反力を推定する推定器を腕メイン制御装置に備えても良い。この推定器は、従来技術である外乱オブザーバーを用いれば良い。
【０２７１】
また、目標対象物反力を零に設定しても良い場合もある。例えば、前記した作業４では手摺りとハンドの間の摩擦がほぼ零であれば、目標対象物反力を零に設定するのが良い。また、作業２で台車の自在キャスタ輪の摩擦が不明ならば、目標対象物反力を零に設定すれば良い。尚、目標対象物反力が常に零である場合では、対象物反力偏差は実対象物反力と一致するので、上記において対象物反力偏差に代えて実対象物反力を用いても良い。
【０２７２】
また、床が平面でない場合でも、本出願人が特開平５−３１８３４０号公報で提案した仮想平面を想定する技術を用い、目標全床反力中心点や目標ＺＭＰを仮想平面上に求めることで、適用が可能となる。
【０２７３】
また、上記において、ブロック図は演算処理順序を変えるなど、種々の変形が可能である。
【０２７４】
【発明の効果】
請求項１項にあっては、移動ロボットの姿勢制御装置において、腕部リンクに実際に作用する対象物からの反力のうちの設定された所定方向の成分である第１の外力に応じ想定外の外力が作用するとき、想定外の外力の、ある所定方向の成分である第１の外力に応じ、所定方向に直交する方向に腕部リンクに第２の外力が作用するように、腕部リンクを駆動して前記ロボットの姿勢を安定させるように制御する如く構成したので、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動バランスを維持して安定な姿勢を保持することができる。
【０２７５】
請求項２項にあっては、移動ロボットの姿勢制御装置において、少なくともロボットの目標腕部リンク位置姿勢と、対象物から腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成し、対象物反力の実際値である実対象物反力を検出あるいは推定し、少なくとも検出あるいは推定された実対象物反力に基づき、ロボットの姿勢が安定するように目標動作のうちの少なくとも前記目標腕部リンク位置姿勢または前記目標対象物反力を修正し、修正された目標動作に基づいて腕部リンクを駆動する腕部駆動装置を備える如く構成したので、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動バランスを維持して安定な姿勢を保持することができる。さらには、対象物の運動に影響を与えることなく、動バランスを維持して安定な姿勢を保持することができる。
【０２７６】
請求項３項にあっては、少なくとも実対象物反力と目標対象物反力との差を示す対象物反力偏差に基づき、ロボットの姿勢が安定するように目標動作を修正する如く構成したので、上記した効果を一層良く得ることができる。
【０２７７】
請求項４項にあっては、対象物反力偏差の第１の成分によって発生する転倒力の全部あるいは一部を、第１の成分から得られる第２の成分によって打ち消すように、目標動作を修正する如く構成したので、上記した効果を一層良く得ることができる。
【０２７８】
請求項５項にあっては、移動ロボットの姿勢制御装置において、少なくともロボットの目標腕部リンク位置姿勢と、対象物から腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成すると共に、ロボットの姿勢傾き偏差を検出し、検出した姿勢傾き偏差が零に近づくように目標動作のうちの少なくとも前記目標腕部リンク位置姿勢または前記目標対象物反力を修正し、修正された目標動作に基づいて腕部リンクを駆動する如く構成したので、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動バランスを維持して安定な姿勢を保持することができる。さらには、対象物の運動に影響を与えることなく、動バランスを維持して安定な姿勢を保持することができる。
【図面の簡単な説明】
【図１】この発明の一つの実施の形態に係る移動ロボットの姿勢制御装置が対象とする脚式移動ロボットの正面図である。
【図２】図１に示すロボットの側面図である。
【図３】図１に示すロボットをスケルトンで示す説明図である。
【図４】図３に示す電子制御ユニット（ＥＣＵ）などの構成を詳細に示すブロック図である。
【図５】この発明の一つの実施の形態に係る移動ロボットの姿勢制御装置の構成を示すブロック図である。
【図６】図５に示す装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。
【図７】図６と同様に、図５に示す装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。
【図８】図５に示す装置の内、脚メイン制御装置に適用される複合コンプライアンス制御装置の構成を示すブロック図である。
【図９】図５に示す装置が前提とする原理の説明図である。
【図１０】図９と同様に、図５に示す装置が前提とする原理の説明図である。
【図１１】図９と同様に、図５に示す装置が前提とする原理の説明図である。
【図１２】図９と同様に、図５に示す装置が前提とする原理の説明図である。
【図１３】図９と同様に、図５に示す装置が前提とする原理の説明図である。
【図１４】図５に示す装置の内、姿勢安定化メイン制御装置の構成を示すブロック図である。
【図１５】図１４に示す姿勢安定化メイン制御装置の内、姿勢安定化用モーメント算出器の構成を示すブロック図である。
【図１６】図１４に示す姿勢安定化メイン制御装置の内、対象物反力偏差モーメント分離器の構成を示すブロック図である。
【図１７】図１４に示す姿勢安定化メイン制御装置の内、対象物反力平衡制御用分配器の構成を示すブロック図である。
【図１８】図１４に示す姿勢安定化メイン制御装置の内、対象物反力平衡制御用分配器の構成を示す、図１７と同様なブロック図である。
【図１９】図１４に示す姿勢安定化メイン制御装置の内、対象物反力平衡制御装置の構成を示すブロック図である。
【図２０】図１４に示す姿勢安定化メイン制御装置の内、補償モーメント分配器の構成を示すブロック図である。
【図２１】図１４に示す姿勢安定化メイン制御装置の内、修正目標対象物反力算出器の構成を示すブロック図である。
【図２２】図５に示す装置の作用および効果を説明するために、制御系を対象物反力に注目して変形しつつ簡略化して示すブロック図である。
【図２３】同様に図５に示す装置の作用および効果を説明するために、制御系を対象物反力に注目して変形しつつ簡略化して示すブロック図である。
【図２４】図１４に示す姿勢安定化メイン制御装置の動作を説明する説明図である。
【図２５】この発明の第２の実施の形態に係る移動ロボットの姿勢制御装置の構成を示す、図１４と類似する、姿勢安定化メイン制御装置の構成を示すブロック図である。
【図２６】この発明の第３の実施の形態に係る移動ロボットの姿勢制御装置の構成を示す説明図である。
【符号の説明】
１ ロボット（脚式移動ロボット）
２ 脚部リンク
３ 基体（上体）
５ 腕部リンク
１０，１２，１４Ｒ，Ｌ 股関節駆動用電動モータ
１６Ｒ，Ｌ 膝関節駆動用電動モータ
１８，２０Ｒ，Ｌ 足関節駆動用電動モータ
２２Ｒ，Ｌ 足部（足平）
３０，３２，３４Ｒ，Ｌ 肩関節駆動用電動モータ
３６Ｒ，Ｌ 肘関節駆動用電動モータ
３８Ｒ，Ｌ 手首関節
４０Ｒ，Ｌ ハンド
６２ 電子制御ユニット（ＥＣＵ）
５６ 力センサ（６軸力センサ）
５８ 力センサ（６軸力センサ）
６０ 傾斜センサ
８８ アクチュエータ駆動装置
９０ 脚アクチュエータ
９２ 腕アクチュエータ
１００ 目標作業パターン生成器
１０２ 脚メイン制御装置
１０４ 腕メイン制御装置
１０６ 姿勢安定化メイン制御装置
１０８ 台車（作業対象物（対象物））
２００ 姿勢安定化用補償モーメント算出器
２０２ 対象物反力平衡制御装置
２０４ 対象物反力モーメント偏差分離器
２０６ 修正目標対象物反力算出器
２０８ 対象物反力平衡制御用分配器（分配器）
２１０ 補償モーメント分配器（分配器）
２１４ 乗算点
２１５ 加算点
２１６ 加算点
３００ ロボット（車輪式の移動ロボット）
３０２ 基体（上体）
３０４ 車輪
３１０ アーム（腕部リンク）
３１２ 電子制御ユニット（ＥＣＵ）

Claims (5)

1. 少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも１個の腕部リンクからなる移動ロボットの姿勢制御装置において、前記腕部リンクに実際に作用する対象物からの反力のうちの設定された所定方向の成分である第１の外力を検出あるいは推定する所定方向成分検出手段を備えると共に、前記所定方向成分検出手段が検出あるいは推定した第１の外力に応じ、前記所定方向に直交する方向に前記腕部リンクに第２の外力が作用するように、前記腕部リンクを駆動して前記ロボットの姿勢を安定させるように制御することを特徴とする移動ロボットの姿勢制御装置。
2. 少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも１個の腕部リンクからなる移動ロボットの姿勢制御装置において、
   ａ．少なくとも前記ロボットの目標腕部リンク位置姿勢と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成する目標動作生成手段、
   ｂ．前記対象物反力の実際値である実対象物反力を検出あるいは推定する実対象物反力検出手段、
   ｃ．少なくとも前記検出あるいは推定された実対象物反力に基づき、前記ロボットの姿勢が安定するように前記目標動作のうちの少なくとも前記目標腕部リンク位置姿勢または前記目標対象物反力を修正する姿勢安定化制御手段、
   および
   ｄ．少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置、
   を備えたことを特徴とする移動ロボットの姿勢制御装置。
3. 前記姿勢安定化制御手段は、少なくとも前記実対象物反力と前記目標対象物反力との差を示す対象物反力偏差に基づき、前記ロボットの姿勢が安定するように前記目標動作を修正することを特徴とする請求項２項記載の移動ロボットの姿勢制御装置。
4. 前記姿勢安定化制御手段は、前記対象物反力偏差の第１の成分によって発生する転倒力の全部あるいは一部を、前記第１の成分から得られる第２の成分によって打ち消すように、前記目標動作を修正することを特徴とする請求項３項記載の移動ロボットの姿勢制御装置。
5. 少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも１個の腕部リンクからなる移動ロボットの姿勢制御装置において、
   ａ．少なくとも前記ロボットの目標腕部リンク位置姿勢と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成する目標動作生成手段、
   ｂ．前記ロボットの姿勢傾き偏差を検出し、前記検出した姿勢傾き偏差が零に近づくように前記目標動作のうちの少なくとも前記目標腕部リンク位置姿勢または前記目標対象物反力を修正する姿勢安定化制御手段、
   および
   ｃ．少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置、
   を備えたことを特徴とする移動ロボットの姿勢制御装置。

Images