WO2003092968A1

WO2003092968A1 - Dispositif de commande d'attitude d'un robot mobile

Info

Publication number: WO2003092968A1
Application number: PCT/JP2003/004990
Authority: WO
Inventors: Toru Takenaka; Tadaaki Hasegawa; Takashi Matsumoto
Original assignee: Honda Giken Kogyo Kabushiki Kaisha
Priority date: 2002-05-01
Filing date: 2003-04-18
Publication date: 2003-11-13
Also published as: US20050104548A1; EP1510302A1; EP1510302B1; US7112938B2; KR20040111534A; DE60328285D1; JP3833567B2; AU2003235263A1; KR100685720B1; KR20060107582A; JP2003326483A; EP1510302A4

Description

移動ロボットの姿勢制御装置技術分野

この発明は移動口ポットの姿勢制御装置に関し、特に、腕部を備える 2足の脚式移動ロボットあるいは車輪式の移動口ポットの姿勢制御装置において、ロボットが腕を介して対象物から反力を受けるような作業を行なっているときの姿勢安定化を図るようにした姿勢制御装置に関する。背景技術

移動ロボットの中の脚式移動ロボットの姿勢制御装置としては、本出願人が先に提案した特開平 1 0— 2 3 0 4 8 5号公報に記載される技術が知られている。その技術においては、対象物から予期せぬ反力を受けると、長期的にはロボットの上体の位置をずらすように姿勢を徐々に修正することによってロボット全体の重心位置をずらし、補償のための重力モーメントを発生させて動バランス、即ち、動力学的に釣り合うバランスを維持すると共に、それまでの過渡期においては、目標床反力中心点（目標 ZMPに一致している）まわりに補償のための全床反力モーメントを発生させるように足部を動かして踏み止めさせることによって、動バランスを維持するように構成している。

このように、上記した従来技術においては、対象物から受ける予期せぬ反力が急激に変化した場合には、その瞬間では（短期的には）、足部を動かして踏み止めさせることによって動バランスを維持する、即ち、全床反力モーメントを発生させることによつて姿勢安定化が図られる。

また、本出願人は、対象物からの反力を受ける受けないに関わらず、ロボットの姿勢が傾いているときに、これを復元させるためのモーメントを全床反力モーメントとして積極的に発生させる技術も提案しているが（特開平 1 0— 2 7 7 9 6 9号公報）、いずれの技術においても、全床反力モーメントには限界があり、それを越えて全床反力モーメントを発生させようとすると、足部の一部が床から浮いてしまい、その結果、ロボットは動バランスを崩し、最悪の場合には転倒する恐れがあった。

また、本出願人は、全床反力モーメントを発生させる代わりに上体の軌道を修正することによって動バランスを維持する技術も提案しているが（特許第 3 2 6 9 8 5 2号公報）、力かる技術にあっては足の着地位置などを修正する必要があるため、腕で対象物に対する作業を行なっているとき、上体や足が作業に適切な位置から離れてしまう場合があった。

また、対象物になんらかの運動を与えるような作業を行なう場合にあっては、作業を所期通り遂行するためには、動パランスを維持したり、あるいは傾いた姿勢の復元を行なっても、対象物の運動に影響を与えないことが望ましい。発明の開示

従って、この発明の第 1の目的は、上記した不都合を解消することにあり、移動口ポットが対象物から反カをうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動バランスを維持して安定な姿勢を保持するようにした移動ロボットの姿勢制御装置を提供することを目的とする。

さらには、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、接地する床から作用される床反力を操作することなく、あるいはその操作量を低減しつつ、動バランスを維持して安定な姿勢を保持するようにした移動ロボットの姿勢制御装置を提供することを第 2の目的とする。

さらには、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、対象物の運動に影響を与えることなく動バランスを維持して安定な姿勢を保持するようにした移動ロボットの姿勢制御装置を提供することを第 3の目的とする。この発明は、上記した目的を達成するために、後述する請求の範囲第 1項に記載する如く、少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも 1個の腕部リンクからなる移動ロボットの姿勢制御装置において、想定外の外力が作用するとき、前記想定外の外力の、ある所定方向の成分である第 1の外力に応じ、前記所定方向に直交する方向に前記腕部リンクに第 2の外力が作用するように、前記腕部リンクを駆動して前記ロボットの姿勢を安定させるように制御する如く構成した。このように、移動ロボットの姿勢制御装置において、想定外の外力が作用するとき、想定外の外力の、ある所定方向の成分である第 1の外力に応じ、所定方向に直交する方向に腕部リンクに第 2の外力が作用するように、腕部リンクを駆動して前記ロボットの姿勢を安定させるように制御する如く構成したので、移動ロボットが対象物から反力をうけるような作業を行つているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反カを受けても、動パランスを維持して安定な姿勢を保持することができる。

また、この発明は、後述する請求の範囲第 2項に記載する如く、少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも 1個の腕部リンクからなる移動ロボットの姿勢制御装置において、少なくとも前記ロボットの目標腕部リンク位置姿勢と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成する目標動作生成手段、前記対象物反力の実際値である実対象物反力を検出あるいは推定する実対象物反力検出手段、少なくとも前記検出あるいは推定された実対象物反力に基づき、前記ロポットの姿勢が安定するように前記目標動作を修正する姿勢安定ィヒ制御手段、および少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置を備える如く構成した。このように、移動ロボットの姿勢制御装置において、少なくともロボットの目標腕部リンク位置姿勢と、対象物から腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成し、対象物反力の実際値である実対象物反力を検出あるいは推定し、少なくとも検出あるいは推定された実対象物反力に基づき、ロボットの姿勢が安定するように目標動作を修正し、修正された目標動作に基づいて腕部リンクを駆動する腕部駆動装置を備える如く構成したので、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動パランスを維持して安定な姿勢を保持することができる。さらには、対象物の運動に影響を与えることなく、動バランスを維持して安定な姿勢を保持することができる。

また、この発明は、後述する請求の範囲第 3項に記載する如く、前記姿勢安定化制御手段は、少なくとも前記実対象物反力と前記目標対象物反力との差を示す対象物反力偏差に基づき、前記ロポットの姿勢が安定するように前記目標動作を修正する如く構成した。このように、少なくとも実対象物反力と目標対象物反力との差を示す対象物反力偏差に基づき、ロポットの姿勢が安定するように目標動作を修正する如く構成したので、上記した効果を一層良く得ることができる。また、この発明は、後述する請求の範囲第 4項に記載する如く、前記姿勢安定ィ匕制御手段は、前記対象物反力偏差の第 1の成分によって発生する転倒力の全部あるいは一部を、前記第 1の成分から得られる第 2の成分によって打ち消すように、前記目標動作を修正する如く構成した。このように、対象物反力偏差の第 1 の成分によって発生する転倒力の全部あるいは一部を、第 1の成分から得られる第 2の成分によって打ち消すように、目標動作を修正する如く構成したので、上記した効果を一層良く得ることができる。

また、この発明は、後述する請求の範囲第 5項に記載する如く、少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも 1個の腕部リンクからなる移動ロボットの姿勢制御装置において、少なくとも前記ロボットの目標腕部リンク位置姿勢と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成する目標動作生成手段、前記ロボットの姿勢傾き偏差を検出し、前記検出した姿勢傾き偏差が零に近づくように前記目標動作を修正する姿勢安定化制御手段、および少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置を備える如く構成した。このように、移動ロボットの姿勢制御装置において、少なくともロボットの目標腕部リンク位置姿勢と、対象物から腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成すると共に、ロボットの姿勢傾き偏差を検出し、検出した姿勢傾き偏差が零に近づくように目標動作を修正し、修正された目標動作に基づいて腕部リンクを駆動する如く構成したので、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動ノを維持して安定な姿勢を保持することができる。さらには、対象物の運動に影響を与えることなく、動パランスを維持して安定な姿勢を保持することができる。尚、特許請求の範囲および発明の詳細な説明欄において、『移動ロボット』は腕部リンク以外に対象物反力を受ける移動口ポットも含む意味で使用する。また、『腕部リンク』に関し、脚部リンクであっても、作業対象物に作用するものであれば、腕部リンクとみなすこととする。例えば、昆虫型の 6個の脚部リンクを備えたロボットにおいて、前の 2個の脚部リンクを用いて物を持ち上げる場合には、その脚部リンクは腕部リンクとみなすこととする。

また、「位置姿勢」は「位置」および「姿勢」の両者を含んでも良く、あるいはそのいずれかであっても良い意味で使用する。換言すれば、「位置姿勢」は、必ず「位置」および「姿勢」の両者を含まなければならないものではない。図面の簡単な説明

第 1図は、この発明の一つの実施の形態に係る移動ロボットの姿勢制御装置が対象とする脚式移動口ポットの正面図である。

第 2図は、第 1図に示すロボットの側面図である。

第 3図は、第 1図に示すロボットをスケルトンで示す説明図である。

第 4図は、第 3図に示す電子制御ユニット（E C U) などの構成を詳細に示すプロック図である。

第 5図は、この発明の一つの実施の形態に係る移動ロボットの姿勢制御装置の構成を示すプロック図である。

第 6図は、第 5図に示す装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。

第 7図は、第 6図と同様に、第 5図に示す装置の目標作業パターン生成器が生成する歩容における支持脚座標系を示す説明図である。

第 8図は、第 5図に示す装置の内、脚メイン制御装置に適用される複合コンプライアンス制御装置の構成を示すプロック図である。

第 9図は、第 5図に示す装置が前提とする原理の説明図である。

第 1 0図は、第 9図と同様に、第 5図に示す装置が前提とする原理の説明図である。

第 1 1図は、第 9図と同様に、第 5図に示す装置が前提とする原理の説明図である。

第 1 2図は、第 9図と同様に、第 5図に示す装置が前提とする原理の説明図である o

第 1 3図は、第 9図と同様に、第 5図に示す装置が前提とする原理の説明図である。

第 1 4図は、第 5図に示す装置の内、姿勢安定化メイン制御装置の構成を示すブロック図である。

第 1 5図は、第 1 4図に示す姿勢安定ィ匕メィン制御装置の内、姿勢安定化用モーメント算出器の構成を示すプロック図である。

第 1 6図は、第 1 4図に示す姿勢安定化メイン制御装置の内、対象物反力偏差モーメント分離器の構成を示すプロック図である。

第 1 7図は、第 1 4図に示す姿勢安定化メイン制御装置の内、対象物反力平衡制御用分配器の構成を示すプロック図である。

第 1 8図は、第 1 4図に示す姿勢安定ィヒメイン制御装置の内、対象物反力平衡制御用分配器の構成を示す、第 1 7図と同様なプロック図である。

第 1 9図は、第 1 4図に示す姿勢安定化メイン制御装置の内、対象物反力平衡制御装置の構成を示すプロック図である。

第 2 0図は、第 1 4図に示す姿勢安定化メィン制御装置の内、補償モーメント分配器の構成を示すブロック図である。

第 2 1図は、第 1 4図に示す姿勢安定化メイン制御装置の内、修正目標対象物反力算出器の構成を示すプロック図である。

第 2 2図は、第 5図に示す装置の作用および効果を説明するために、制御系を対象物反力に注目して変形しつつ簡略ィ匕して示すプロック図である。

第 2 3図は、同様に第 5図に示す装置の作用おょぴ効果を説明するために、制御系を対象物反力に注目して変形しつつ簡略ィ匕して示すプロック図である。

第 2 4図は、第 1 4図に示す姿勢安定化メィン制御装置の動作を説明する説明図である。第 2 5図は、この発明の第 2の実施の形態に係る移動ロボットの姿勢制御装置の構成を示す、第 1 4図と類似する、姿勢安定化メイン制御装置の構成を示すブ口ック図である。

第 2 6図は、この発明の第 3の実施の形態に係る移動ロボットの姿勢制御装置の構成を示す説明図である。発明を実施するための最良の形態

以下、添付図面を参照してこの発明の一つの実施の形態に係る移動ロボットの姿勢制御装置を説明する。

第 1図はこの実施の形態に係る移動ロボットの姿勢制御装置が対象とする脚式移動口ポットの正面図、第 2図はその側面図である。尚、移動ロボットとしては、 2個の脚部リンクと 2個の腕部リンクを備えたヒューマノイド型（人間型）の脚式移動ロボットを例にとる。

第 1図に示すように、脚式移動ロボット（以下「ロボット」という） 1は、複数個（本）、より具体的には 2個（本）の脚部リンク（あるいは脚） 2を備えると共に、その上方には上体（基体） 3が設けられる。上体 3のさらに上方には頭部 4が形成されると共に、上体 3の両側には 2個（本）の腕部リンク（あるいは腕） 5が連結される。また、第 2図に示すように、上体 3の背部には格納部 6が設けられ、その内部には電子制御ユエット（後述）などが収容される。尚、第 1 図おょぴ第 2図に示すロボット 1は、内部構造を保護するためのカバーで被覆される。

第 3図はロボット 1をスケルトンで示す説明図である。同図を参照してその内部構造を関節を中心に説明すると、図示の如く、ロボット 1は、左右それぞれの脚部リンク 2および腕部リンク 5に、 1 1個の電動モータで動力化された 6個の関節を備える。

即ち、ロボット 1は、腰部（股部）に、脚部リンク 2を口、直軸（Z軸あるいは鉛直軸）まわりに回転させる関節を駆動する電動モータ 1 O R, 1 0 L (右側を R、左側を Lとする。以下同じ）と、脚部リンク 2をピッチ（進行）方向（Y軸まわり）に摇動させる関節を駆動する電動モータ 1 2 R, 1 2 Lと、脚部リンク 2をロール（左右）方向（X軸まわり）に回転させる関節を駆動する電動モータ 14R, 14 Lを備えると共に、膝部に脚部リンク 2の下部をピッチ方向（Y軸まわり）に回転させる膝関節を駆動する電動モータ 1 6 R， 1 6 Lを備え、さらに足首に脚部リンク 2の先端側をピッチ方向（Y軸まわり）に回転させる足（足首）関節を駆動する電動モータ 1 8 R, 1 8 Lとロール方向（X軸まわり）に回転させる足（足首）関節を駆動する電動モータ 20 R， 20 Lを備える。

上記したように、第 3図において、関節はそれを駆動する電動モータ（あるいは電動モータに接続されてその動力を伝動するプーリなどの伝動要素）の回転軸線で示す。尚、脚部リンク 2の先端には足部（足平） 22 R, 22 Lが取着される。

このように、脚部リンク 2の股関節（腰関節）には電動モータ 1 OR (L) ， 1 2R (L) , 1 4R (L) がそれらの回転軸線が直交するように配置されると共に、足関節（足首関節）には電動モータ 1 8R (L) , 2 OR (L) がそれらの回転軸線が直交するように配置される。尚、股関節と膝関節は大腿リンク 24 R (L) で、膝関節と足関節は下 Ji退リンク 26R (L) で連結される。

脚部リンク 2は股関節を介して上体 3に連結されるが、第 3図では上体 3を上体リンク 28として簡略的に示す。前記したように、上体 3には腕部リンク 5が連結される。

腕部リンク 5も、脚部リンク 2と同様に構成される。即ち、ロボット 1は、肩部に、腕部リンク 5をピッチ方向に回転させる関節を駆動する電動モータ 3 OR ， 30 Lとロール方向に回転させる関節を駆動する電動モータ 3 2 R, 3 2 Lを備えると共に、その自由端側を回転させる関節を駆動する電動モータ 34 R, 3 4 Lと、肘部にそれ以降の部位を回転させる関節を駆動する電動モータ 3 6 R, 36 Lを備え、さらにその先端側にそれを回転させる手首関節を駆動する電動モータ 38R， 3 8 Lを備える。尚、手首の先にはハンド（エンドェフエクタ） 4 OR, 40 Lが取着される。

即ち、腕部リンク 5の肩関節には電動モータ 3 OR (L) , 3 2R (L) , 3 4R (L) がそれらの回転軸線が直交するように配置される。尚、肩関節と肘関節とは上腕リンク 42R (L) で、肘関節と手首関節とは下腕リンク 44R (L ) で連結される。

また、頭部 4は、鉛直軸まわりの首関節 4 6と、それと直交する軸まわりに頭部 4を回転させる頭部揺動機構 4 8を介して上体 3に連結される。第 3図に示す如く、頭部 4の内部には撮像した画像を示す信号を出力する、 C C Dカメラからなる視覚センサ 5 0が配置されると共に、レシーバおよびマイクロフォンからなる音声入出力装置 5 2が配置される。

上記の構成により、脚部リンク 2は左右の足について 6個の関節を備えて合計 1 2の自由度を与えられ、 6個の関節を適宜な角度で駆動（関節変位）することで、脚部リンク 2に所望の動きを与えることができ、ロボット 1を任意に 3次元空間を歩行させることができる。また、腕部リンク 5も左右の腕について 5個の関節を備えて合計 1 0の自由度を与えられ、 5個の関節を適宜な角度で駆動（関節変位）することで所望の作業を行わせることができる。さらに、頭部 4は 2つの自由度からなる関節あるいは揺動機構を与えられ、これらを適宜な角度で駆動することにより所望の方向に頭部 4を向けることができる。

1 O R ( L) などの電動モータのそれぞれにはロータリエンコーダ（図示せず ) が設けられ、電動モータの回転軸の回転を通じて対応する関節の角度、角速度、および角加速度の少なくともいずれかを示す信号を出力する。

足部 2 2 R ( L) には公知の 6軸力センサ（以下「力センサ」という） 5 6が取着され、ロポットに作用する外力の内、接地面からロボット 1に作用する床反力の 3方向成分 F X , F y , F zとモーメントの 3方向成分 M x， M y , M zを示す信号を出力する。

また、手首関節とハンド 4 O R ( L) の間には同種の力センサ（6軸力センサ ) 5 8が取着され、口ポット 1に作用する床反力以外の外力、具体的にはハンド 4 O R ( L) に対象物から作用する外力（対象物反力）の 3方向成分 F x , F y ， F zとモーメントの 3方向成分 M x， My , M zを示す信号を出力する。

さらに、上体 3には傾斜センサ 6 0が設置され、鉛直軸に対する上体 3の傾き (傾斜角度）とその角速度の少なくともいずれ力、即ち、ロボット 1の上体 3の傾斜（姿勢）などの状態量を示す信号を出力する。

これら力センサ 5 6などの出力群は、ロボット 1の上体 3の背中側の格納部 6 (第 2図に示す）に収容された、マイクロコンピュータからなる電子制御ュニット Electric Control Unit。以下「ECU」とレ、う） 62に送られる（図示の便宜のためロボット 1の右側についてのみ図示する）。

第 4図は ECU 62の詳細を示すプロック図であり、マイクロ ·コンピュータから構成される。そこにおいて傾斜センサ 60などの出力は AZD変 l 70でデジタル値に変換され、その出力はパス 72を介して RAM74に送られる。また各ァクチユエータにおいて電動モータに隣接して配置されるエンコーダの出力は、カウンタ 76を介して RAM74内に入力される。

ECU62内には CPUからなる演算装置 80が設けられており、演算装置 8 0が生成した歩容に基づいてロボットが安定な姿勢を継続することができるように、関節角変位指令（ァクチユエータ変位指令）を算出し、 RAM74に送出する。尚、符号 84は ROMを示す。

また、演算装置 80は RAM74からその指令と検出された実測値とを読み出し、各関節の駆動に必要な制御値（操作量）を算出して DZA変換器 86と各関節に設けられたァクチユエータ駆動装置（アンプ） 88を介して各関節を駆動する、変位検出器（エンコーダ）付きの脚ァクチユエータ 90と腕ァクチユエータ 92の電動モータ 1 OR (L) ， 12R (L) などに出力する。

第 5図は、この実施の形態に係る脚式移動ロボットの姿勢制御装置（主として前記した演算装置 80に相当）の構成およぴ動作を機能的に示すプロック図である。

この装置は脚おょぴ腕の動作を統合的に制御する装置であり、後述するように操作量を算出して各ァクチユエータ駆動装置 88にァクチユエータ変位指令を出力し、脚ァクチユエータ 90と腕ァクチユエータ 92を動作させる。図示の如く、この装置は、目標作業パターン生成器 100、脚メイン制御装置 102、腕メイン制御装置 104、および姿勢安定ィ匕メイン制御装置 106などから構成される。

目標作業パターン生成器 100は、ある想定条件下においてロボット 1の動力学的平衡条件を満足する歩容を含む、目標作業パターンを生成する。目標作業パターンは、複数の変数の時間変化パターンによって表現される。この変数は、運動を表現する変数と環境から受ける反力を表現する変数から構成される。

ここで、運動を表現する変数は、これによつて各瞬間における姿勢が一義的に決定できる変数の組である。具体的には、目標足部位置姿勢、目標上体位置姿勢、目標ハンド位置姿勢から構成される。

また、環境から受ける反力を表現する変数は、後述する目標全床反力中心点、目標反力おょぴ目標対象物反力から構成される。

これら各変数は、支持脚座標系で表される。支持脚座標系は、支持脚足首（足関節を駆動する電動モータ 1 8 , 2 O R ( L) の回転軸線の交点）から足部 2 2 R ( L) への垂直投影点を原点とする座標系であり、第 6図および第 7図に示すように、支持脚が接地している床に固定された座標系であり、支持脚足部の前向きを X軸の向き、左向きを Y軸の向き、鉛直方向上向きを Z軸向きとする座標系である。

以下、これら各変数を詳細に説明する。

先ず、「対象物反力」とは、ロボット 1が環境から受ける外力の内で、各足部床反力を除いた外力を意味する。具体的には、ハンド 4 O R ( L ) が作業対象物 (例えばドアの把手）力 ^受ける反力を意味する。尚、その目標値を「目標対象物反力」と呼ぶ。

目標作業パターン生成器 1 0 0が出力する目標対象物反力は、後述する目標全床反力中心点まわりに作用する力とモーメントによって表現される。ちなみに、姿勢安定化にとって重要なのは、このうちのモーメント成分である。

目標全床反力と目標全床反力中心点（位置）について説明すると、作業中において各足部が床から受けるべき目標床反力の合力を「広義の目標全床反力」と呼ぶ。広義の目標全床反力は、作用点とその点における力とモーメントで表現される。目標全床反力中心点（位置）は、目標全床反力をその点を作用点とする力とモーメントで表現したとき、 X軸まわりモーメント成分と Y軸まわりモーメント成分が零（0 ) になる床面上の点である。

狭義の目標全床反力は、広義の目標全床反力を、目標全床反力中心点を作用点として、力とモーメントで表現した場合の力とモーメントを意味する。目標作業パターン生成器が出力する目標全床反力は、狭義の目標全床反力である。 0

12 以降は特に説明がない限り、目標全床反力は、狭義の目標全床反力を指す。尚、平坦な床面を歩行する場合には、目標全床反力中心点は、通常、その床面上に ¾ れる。

歩行制御の分野において従来から公知である、 M. Vuk o b r a t o v i c によって提唱された ZMPの概念も、次のように拡張する。即ち、ロボット 1の運動によつて生じる慣性力と重力と対象物反力の合力が、その点を作用点とする力とモーメントで表現されたとき、 X軸まわりモーメント成分と Y軸まわりモーメント成分が 0になる床面上の点を「ZMP」と呼ぶ。ロボット 1が目標の運動を行うときの ZMPを「目標 ZMP」と呼ぴ、その位置を「目標 ZMP位置」と呼ぶ。

目標作業パターンが動力学的平衡条件を満足するということは、目標作業バターンによって生じる上記の慣性力と重力と対象物反力の合力と目標全床反力が、打ち消し合って零になることである。従って、動力学的平衡条件を満足するためには、目標全床反力中心点（位置）と目標 ZMP (位置）が一致しなければならない。

目標作業パターン生成器 100では、後述する大局安定ィ匕制御が働いていない場合、動力学的平衡条件を満足する歩容を含む目標作業パターンを生成する。換言すれば、この場合、目標作業パターン生成器 100が生成する目標全床反力中心点（位置）は目標 ZMP (位置）に一致する。

目標足部位置姿勢、目標上体位置姿勢、目標ハンド位置姿勢は、前記した支持脚座標系で表現されたそれぞれの部位の位置と姿勢を表す。具体的には、上体 3 の位置おょぴその速度は、上体 3の重心位置などの代表点おょぴその（変位）速度を意味する。更に、上体 3あるいは足部 22R (L) の姿勢は、 X, Y， Ζ空間における向きを意味する。

次いで、脚メィン制御装置 102について説明する。脚メィン制御装置 102 の機能は、概説すると、脚ァクチユエータ（10R (L) などの電動モータなど ) を操作し、目標姿勢に追従する姿勢追従制御と、目標全床反力と補償全床反力モーメント（後述）の合力に追従する床反力制御を同時に行う装置である。尚、目標床反力と補償全床反力モーメントの合力と目標姿勢を同時に完全に満足させることは不可能であるので、適当な調整が行われ、長期的には両方を満足するように制御される。

より詳しくは、脚メイン制御装置 1 0 2は、修正目標上体位置姿勢（後述）、目標足部位置姿勢、目標全床反力中心点（位置）と、その中心点に作用する目標全床反力と、補償全床反力モーメントを入力し、目標全床反力中心点（位置）に作用する実全床反力のモーメント成分が、目標全床反力モーメント（通常は零）と補償全床反力モーメントの和に一致するように、目標足部位置姿勢を修正する

。修正された目標足部位置姿勢を「修正目標足部位置姿勢」と呼ぶ。

脚メイン制御装置 1 0 2は、この修正目標上体位置姿勢と修正目標足部位置姿勢から決定される目標脚関節変位に実関節変位が追従するように、ァクチユエ一タ変位指令を出力して脚ァクチユエータ 9 0を制御する。

以上の機能を実現させるために、この実施の形態にあっては、前記した特開平

1 0 - 2 7 7 9 6 9号公報に記載される技術（以下「複合コンプライアンス制御

」という）を適用する。この複合コンプライアンス制御を行う装置は、脚メイン制御装置の他に、足部 2 2 R ( L) に設けた力センサ 5 6、ァクチユエータ駆動装置 8 8およぴァクチユエータ 9 0を包含して構成される。

第 8図に複合コンプライアンス制御装置のプロック図を示すが、その詳細な説明は上記特開平 1 0— 2 7 7 9 6 9号公報に記載されているので省略する。尚、第 8図に示す複合コンプライアンス制御装置においては、特開平 1 0— 2 7 7 9 6 9号公報に記載される複合コンプライアンス制御装置と比較すると、上体傾斜フィードバック系の姿勢安定ィ匕制御（補償全床反力モーメントの演算）が省かれている。

その理由は、同様の処理が後述の姿勢安定ィ匕メイン制御装置で行なわれ、そこで求められた補償全床反力モーメントが入力されるためである。伹し、上体傾斜偏差による床反力の変動を抑制するため、上体傾斜フィードパック系を追加しても良い。

また、この実施の形態において複合コンプライアンス制御に、上記した特許第 3 2 6 9 8 5 2号公報に記載される技術（以下、「大局安定化制御」という）を加えることとする。但し、大局安定ィ匕制御は上体の位置や歩幅を修正、より具体的には上記した修正目標上体位置姿勢をさらに修正するので、後述の腕部リンク

5の制御にあたり、ハンド 4 O R ( L) と対象物との相対位置関係が重要な場合には、修正される上体の位置や歩幅の影響を考慮する必要がある。また、大局安定ィ匕制御が働くと、目標作業パターンにおける目標 Z MPは、目標全床反力中心点から故意にずらされた点に制御される。

大局安定化制御が加えられることで修正目標上体位置姿勢がさらに修正されること力ら、それによつて最終的に得られる目標上体位置姿勢を、第 5図に示す如く、「最終修正目標上体位置姿勢」と呼ぶ。尚、大局安定化制御を加えることはこの発明において必須ではない。即ち、大局安定化制御を加えなくても良く、その場合は、修正目標上体位置姿勢を最終修正目標上体位置姿勢として扱えば足りる。

尚、第 5図において、脚メイン制御装置 1 0 2で修正された最終修正目標足部位置姿勢は、姿勢安定ィ匕メイン制御装置 1 0 6に入力される。伹し、姿勢安定ィ匕メイン制御装置 1 0 6において、目標足部位置姿勢が修正されたことによるロボットの重心位置の変化が無視できるならば、最終修正目標足部位置姿勢を姿勢安定ィ匕メィン制御装置 1 0 6に入力させる必要はない。

次いで腕メィン制御装置 1 0 4について説明する。腕メィン制御装置 1 0 4の機能は、概説すると、腕ァクチユエータ（3 O R ( L ) などの電動モータなど） 9 2を操作し、目標姿勢に追従する姿勢制御と修正目標対象物反力（後述）に追従する対象物反力制御を同時に行うことである（ここでの「姿勢」は腕部リンク 5の全関節の変位の組を表す）。目標姿勢と修正目標対象物反力を同時に完全に満足させることは不可能であるので、適宜な手法、例えば、従来からマニピユレータのコンプライアンス制御、いわゆる仮想コンプライアンス制御として知られるものを用いる（機械工学便覧、エンジニアリング編、 C 4— 1 0 0頁）。

具体的な制御系構成とァルゴリズムを以下に説明すると、腕メイン制御系は腕メィン制御装置 1 0 4の他に、前記したハンド 4 0 R (L) に設けたカセンサ 5 8、ァクチユエータ駆動装置 8 8および腕ァクチユエータ 9 2を包含して構成される。

腕メイン制御装置 1 0 4は、最終目標上体位置姿勢（あるいは修正目標上体位置姿勢）、目標ハンド位置姿勢および修正目標対象物反力を入力し、力センサ⁵

8によって検出される実対象物反力と修正目標対象物反力の差に応じて目標ハンド位置姿勢を修正する。修正された目標ハンド位置姿勢を「最終修正目標ハンド位置姿勢」と呼ぶ。そして、最終修正目標上体位置姿勢（あるいは修正目標上体位置姿勢）と最終修正目標ハンド位置姿勢から決定される目標腕関節変位に実関節変位が追従するように腕ァクチユエータ 9 2を制御する。

次いで姿勢安定化メイン制御装置 1 0 6について説明する。姿勢安定化メイン制御装置 1 0 6は、動パランスあるいは姿勢パランスをとるために動力学的平後 ΐ 条件を考慮しながら制御を行う。そこで、先ず、装置の概要を説明する前に、動力学的平衡条件について以下に説明する。

実際のロボット 1の姿勢傾きの挙動を決定する最も大きな要因は、目標全床反力中心点（即ち、目標 Ζ ΜΡ ) まわりでの実際の力のモーメントのバランスである。

目標全床反力中心点まわりに作用する力のモーメントを以下に列挙する。

1 ) 慣性力モーメント

2 ) 重力モーメント

3 ) 全床反力モーメント

4 ) 対象物反力モーメント

1貧性力モーメントは、目標全床反力中心点まわりのロボットの角運動量の変化によって生じるモーメントである。この値はオイラー方程式によって求められ、具体的には目標全床反力中心点まわりのロボット 1の角運動量の 1階微分値の符号を反転させたものである。目標作業パターンの慣性力モーメントを「目標慣性力モーメント」と呼ぶ。実際のロボット 1が作業しているときの慣性力モーメントを「実慣性力モーメント」と呼ぶ。重力モーメントは、ロボット 1の重心に作用する重力が目標^^反力中心点まわりに作用するモーメントである。

2個の足部 2 2 R ( L) にそれぞれ作用する床反力の合力を「全床反力」と呼ぶ。全床反力モーメントは、全床反力が目標全床反力中心点まわりに作用するモ一メントである。

前述のように、ハンド 4 O R ( L) が対象物から受ける反力を「対象物反力」と呼ぶ。対象物反力モーメントは、対象物反力が目標全床反力中心点まわりに作用するモーメントである。

さて、理想的な脚メィン制御装置により、ロボット 1が目標作業パターンの運動パターンに忠実に追従していたと仮定する。このときには実慣性力モーメントは目標慣性力モーメントに一致し、実重力モーメントは目標重力モーメントに一致する。

一方、動力学の法則（オイラー方程式）により、実慣性力モーメントと実重力モーメントと実全床反力モーメントと実対象物反力モーメントの和は、必ず零である。

故に、ロポット 1が忠実に目標作業パターンの運動パターン通りに動くためには、目標慣个生力モーメントと目標重力モーメントと実全床反力モーメントと実対象物反力モーメントの和は、零でなければならない。これを条件 1とする。

ところが、実際には実対象物反力モーメントが目標対象物反力モーメントと一致せず、差が生じる。例えば、第 9図に示すように、台車（対象物） 1 0 8を押す作業を行っているとき、台車 1 0 8の実際のころがり摩擦力の絶対値が想定していた ί直よりも突然小さくなってしまった状況である。

この状況では、実対象物反力が目標全床反力中心点の Υ軸まわりに作用するモ一メントは、目標対象物反力が目標全床反力中心点の Υ軸まわりに作用するモーメントよりも正の向きに大きくなつて条件 1を満たさなくなり、ロボット 1は前傾する。尚、モーメントの向きは、座標軸の正方向に向いてロボット 1を時計まわりに回転させるモーメントを正とする。

このような状況においても条件 1を満足させるためには、先に提案した技術では次の 2通りの手法が考えられた。

手法 1 ) 上記偏差を打ち消すように、実全床反力モーメントを変える。具体的には、目標 ^^反力中心点まわりに負の床反力モーメントを発生するように脚メイン制御装置 1 0 2に指令し、脚メイン制御装置 1 0 2においてァクチユエータ変位指令を行って足部 2 2 R ( L) のつまさきを下げ、実 ^^反力モーメントを負の向きに増加させる、即ち、脚部リンク 2.で踏み止めさせるような姿勢をとらせる。手法 2 ) 上記偏差を打ち消すように、目標作業パターンの運動パターンを修正することにより、目標慣性力モーメントと目標重力モーメントを修正する。具体的には、目標上体位置および/または姿勢を修正することにより、目標慣'生カモ一メントと目標重力モーメントを修正する。即ち、上体 3を前後方向に移動させる。

実全床反力モーメントは、目標全床反力モーメントを変えるだけで脚メィン制御装置によってすばやく変化させることができるので、手法 1は短期的な対応に向いている。但し、実全床反力モーメントを大きく変化させると、足部 2 2 R ( L ) の接地圧分布が偏って接地感が減少し、最悪の場合には足部 2 2 R ( L) の一部が浮いてしまう。従って、長期的には、なるべく元の目標全床反力モーメントに戻すべきである。

実全床反力モーメントを元の目標全床反力モーメントに戻すためには、重心位置をずらし（この場合は後方にずらし）、目標重力モーメントによって上記偏差を打ち消すように、手法 2によって目標作業パターンの運動パターンを修正すれば良い。但し、重心位置を急激にずらすと、過大な目標慣性力モーメントが逆向きに発生するので、ゆっくりと重心位置をずらす必要がある。従って、手法 2は長期的な対応に向いている。

従来技術で述べた、本出願人が先に提案した特開平 1 0— 2 3 0 4 8 5号公報記載の技術は、これら両方の手法を同時に行い、短期的には手法 1を主に使うことによつて速レ、変化に対応し、長期的には手法 2を主に使うことによつて実全床反力モーメントを元の目標^反力モーメントに収束させるものであった。

これに対し、この発明に係る姿勢制御装置においては、さらに、実対象物反力の成分の内の拘束方向成分を意図的に変化させる手法（手法 3 ) を追加するようにした。

以下、手法 3の原理について説明する。先ず、理解の便宜のため、 1個（本）の腕部リンクを備えた脚式移動ロボットが、その腕部リンクの先端に配置されたハンドを用いてある対象物に対して作業を行っている場合を考える。このとき、ハンドは対象物から拘束力を受け、拘束運動をするものとする。

対象物がハンドによって空中に支持され、ハンド以外のものによつて支持されないならば、ハンドの運動には 3次元の平行移動の自由度と 3次元の回転の自由度の合計 6つの自由度があり、対象物からは何等拘束力を受けない。

これに対し、対象物がハンド以外のものからも支持されるならば、場合によつては、ハンドの運動はハンド以外の支持されるものによって拘束力を受ける。具体的には、ハンドが対象物、例えば静止した剛性の高い表面に接触すると、ハンドはその面を貫いて動くことはできず、故に自然な位置の拘束が生じる、換言すれば、その面から拘束力を受けることとなる。この発明に係る姿勢制御装置は、その拘束力に着目し、ロボットの姿勢安定ィ匕を図る制御を行うようにした。

説明を続ける前に、ここで、説明に用いる座標系、各種空間や方向を以下のように定義する。尚、以下に記載する変数は、特に、ことわりがない限り、支持脚座標系で表されるものとする。

また、ハンドのある瞬間における X, Yおよび Z方向への速度成分をそれぞれ Vx, Vy, V zとする。同じ瞬間における X， Yおよび Z軸まわりの回転速度成分をそれぞれ ω X, co y, ω zとする。回転は、座標軸の正の方向に向いて時計まわりに回転する向きを正とする。

この瞬間におけるハンドの速度を、ベクトル（Vx， Vy, V z , ω χ, co y , ω ζ) で表わすこととし、これを「ハンド速度ベクトル」と呼ぶ。

そして、任意の実数 Vx, Vy, V z , ω χ， co y, ω ζを要素とするハンド速度ベクトル（Vx, Vy, V z , ω x, co y, ω ζ) の集合を「全運動速度空間」と呼ぶ。全運動速度空間は、いわゆるべクトル空間になっている。

また、ある瞬間に実現不可能なハンド速度ベクトル全ての集合を、この瞬間の「拘束速度領域」と呼ぶ。ここでいう実現不可能なハンド速度ベクトルは、幾何学的な観点から判定される。つまり、この瞬間にハンドと対象物の間の干渉により、ほとんどハンドの変位を生じることなく応力が発生するハンド速度べクトルを、実現不可能なハンド速度ベクトルと定義する。このとき、腕ァクチユエータや腕の関節配置などに因る制約は考えない。

即ち、ハンド自身は、対象物がなければ任意の速度で移動可能であることを前提とする。また、このとき、ハンドおよび対象物が十分な剛性を備えていれば、剛体とみなす。従って、あるハンド速度ベクトル（Vx l , Vy 1 , V z 1 , ω T/JP03/04990

19 x 1, ω γ ΐ, ω z 1) が拘束速度領域の要素ならば、任意の正の実数 kを乗じた（kVx l， k Vy 1, k V z 1, k ω 1 , k ω y 1, k ω z 1) も拘束速度領域の要素とする。尚、便宜上、零ベクトル（0， 0， 0， 0， 0, 0) も拘束速度領域の要素とする。

ここで注意すべき点は、拘束速度領域を、拘束条件下での移動可能な速度領域と混同しないことである。また、拘束速度領域は、いわゆるベクトル空間とは限らない。例えば、床面に載置された十分に硬い対象物をハンドで把持した場合、ハンドを真下（床面の法線方向下方）に下げることはできないが、逆方向（法線方向上方）には移動することができる。

つまり、真下方向へのハンド速度ベクトルは拘束速度領域の要素であるが、その逆方向は拘束速度領域の要素になっていない。拘束速度領域がベタトル空間であれば、拘束速度領域の任意の要素（Vx l, Vy 1, V z 1, ω χ 1, ω y 1 , ω ζ 1) に対し、逆べクトル（一 Vx 1，一Vy 1, — V z 1 , — ω χ 1， - ω y 1, 一 ω ζ ΐ) も拘束速度領域の要素でなければならないが、この場合の拘束速度領域は、この条件を満足しないので、ベクトル空間ではない。

次に、拘束速度空間を以下のように定義する。即ち、拘束速度領域の任意のベクトノレ（Vx l， Vy 1 , V z 1 , ω 1 , ω y 1 , ω ζ 1 ) と逆べクトノレ（一 V 1, -Vy 1, 一 V z l， -ω 1 , -ω y 1, 一 ω ζ ΐ) の集合を、拘束速度空間と定義する。通常は、拘束速度空間は、全運動速度空間の部分ベクトル空間になっている。

全運動速度空間内のべクトルの内で、拘束速度領域の任意のベタトルとの内積が零である（即ちそのベクトルと直交する）全てのベクトルの集合を「自由速度空間」と呼ぶ。

ここで、拘束速度空間および自由速度空間内のベタトルを次のように定義する。拘束速度空間の要素である方向ベクトルを「拘束方向ベクトル」と呼び、単に拘束方向というときは、任意の拘束方向ベクトルの向きを指す。また、自由速度空間の要素である方向ベクトルを「自由方向ベクトル」と呼ぴ、単に自由方向とレヽうときは、任意の自由方向ベクトルの向きを指す。

以下にいくつかの対象物に対する作業に関して拘束速度空間を示す。作業 1 ) 対象物をハンドによって空中に持ち上げている場合

この場合、拘束速度空間は存在しない。尚、便宜上、拘束速度空間は零べクトルだけからなる空間とする。自由速度空間は全運動速度空間に一致する。

作業 2) 前記した第 9図に示す如く、自在キャスタ輪が付いた台車 108をハンド 40 Rで把持して押す場合

この場合、拘束速度空間は、任意の実数 Vz， ω X, coyを持つハンド速度べクトノレ（0， 0, V z , ωχ, ω y, 0) の集合である。

作業 3) 第 10図に示す如く、ハンド 4 ORで把手 1 10を把持しながら、ヒンジ 1 12が付いたドア 1 14を開閉する場合

この場合、ヒンジ軸 1 1 2 Zまわりの回転以外の運動は拘束される。ヒンジ軸を Z軸とし、この瞬間のハンドの X座標を 0、 Y座標を一 r (rはドア 1 14の回転半径、より詳しくは把手 110の回転半径）とすると、自由速度空間は、任意の実数 Vxを持つハンドの速度ベクトル（Vx, 0, 0， 0, 0, Vx/r ) の集合である。

また、拘束速度空間は、任意の実数 Vx， Vy, Vz, ω χ, coyを持つハンド速度ベクトル（Vx， Vy, V ζ, ω x, coy, — r *Vx) の集合である。作業 4) 第 1 1図に示すような環境において、円柱状の手摺り 120にハンド 40Rを滑らせながら階段 122を昇降する場合

この場合、手摺り 120の軸方向の平行移動と、手摺り 120の軸まわりの回転以外の運動は拘束される。手摺り 120の軸が式 x = z + c， y=-d (cおょぴ dは定数）で表わされるとすると、自由速度空間は、任意の実数 Vxおよび ω Xを持つハンド速度べクトノレ (V , 0， Vx, ω χ, 0, ω χ) である。また、拘束速度空間は、任意の実数 Vx， Vy, ω x, coyを持つハンド速度べクトル (Vx, Vy, -Vx, ω χ， ω y, -ω χ) の集合である。尚、理解の便宜のため、手摺り 120と X軸のなす角度を 45度とする。

作業 5) 第 1 1図に示す環境において、手摺り 120をハンド 40Rでしつかり把持して階段 122を昇降する場合 '

手摺り 120をしつかりと把持しているときには運動の自由度はなく、全ての運動が拘束される。従って、拘束速度空間は、全運動速度空間に一致する。上記のような作業において、ハンドから対象物に対してハンドの拘束方向に力を作用させたとき、対象物にはハンド以外の支持するものからその力を打ち消す拘束力しか発生せず、自由方向に実質的に摩擦力などが発生しないならば、ハンドぉよび対象物の運動は変化しなレ、。

作業 3 (第 1 0図）を例にとると、ハンド 4 O Rによってドア 1 1 4に上向き ( Z軸上方）の力を作用させても、ヒンジ 1 1 2の摩擦が実質的に零であるならば、ヒンジ 1 1 2からドア 1 1 4に前記上向きの力を打ち消す力だけが作用するので、ドア 1 1 4の回転運動に何の影響も与えない。

この発明に係る姿勢制御装置は、かかる性質を利用することで、対象物の運動制御に干渉せずに、ロボットの姿勢を安定ィ匕しょうとするものである。即ち、対象物からハンドに作用する力の拘束方向成分を制御することにより、対象物の運動に影響を与えることなく、ロボットの動パランスを維持したり、傾!/、た姿勢を復元するように構成した。

以下に、作業 3を例にとり、この作用について説明する。前述した第 1 0図に示すように、ロボット 1がドア 1 1 4の前に立ってその開閉作業を行っているとき、ロボット 1が前に倒れそうになった状況を考える。口ポット 1の姿勢を復元させるための一つの方法は、ドア 1 1 4を前に押すことである。これによつて口ポット 1はドア 1 1 4から反力を受け、姿勢が復元する。

し力し、姿勢安定化制御のためにドア 1 1 4を押してしまうと、ドア 1 1 4は加速され、その運動は望みの開閉運動からずれてしまうので、あまり大きな復元力を発生させることはできない。特に、ロボット 1の質量に較べてドアの質量が非常に小さい場合には、ほとんど復元力を発生させることができない。また、ドァ 1 1 4の運動が所期の予定から変更されるため、作業遂行上望ましくない。ロボット 1の姿勢を復元させるためのもう一つの方法は、ドア 1 1 4を下に押すことである。これによつてロボット 1はドア 1 1 4から反力を受け、姿勢が復元する。さらに、拘束方向に押しているだけなので、前述のようにドアの運動は変わらない。

この発明に係る姿勢制御装置は後者の方法を用いるものであるため、前記作業 2、作業 3および作業 4のようにハンドの運動に何らかの拘束方向が存在することが、この発明を適用するための必要条件になる。従って、前記作業 1に対しては適用することができない。

尚、拘束速度空間などを表す座標系は、円筒座標系や極座標系などでも良い。通常の座標系なら何を用いてもそれらの間で変換も可能であると共に、その瞬間における拘束速度空間が指す空間に相違はないためである。作業に応じて座標系を適宜設定することで、作業中、拘束速度空間を一定（普遍）にすることができる場合もある。例えば、前記作業 3では、円筒座標系を用いてヒンジ 1 1 2の軸を円筒座標系の軸に一致させることで、各瞬間における拘束速度空間を一定に保つことができる。

次いで、 n個（本）の腕を備える口ポットが作業を行う場合について考える。この場合、各種空間や方向の定義を、次のように拡張する。

まず、下式のように、各ハンドに 1から nまでの識別番号をつけ、それぞれを

「第 jハンド」 ( j = 1 , 2 , · · ·，！!）と呼んで区別する

座標系は前記した支持脚座標系を用いることとし、第 jハンドのある瞬間における X， Yおよび Z方向への速度成分を、それぞれ V x j , V y j , V z j とする。また、同じ瞬間における X, Yおよび z軸まわりの回転速度成分を、それぞれ ω X j , ω y ] , ω ζ ] とする。

そして、ベクトノレ（V χ 1 , V y 1 , V ζ 1 , ω 1 , ω y 1 , ω ζ 1 , V x 2 , V y 2 , V ζ 2 , ω χ 2 , ω y 2 , ω ζ 2 , · · · , V χ η , V y η , V ζ η , ω χ η , ω y η , ω ζ η ) を「全ノヽンド速度べクトノレ」と呼ぶ。このべクトルは、第 1ハンドから第 ηハンドまでのハンド速度べクトルの各要素を順に並べたものである。尚、このベクトル表現の代りに、 j行目が第 jハンドの速度べクトルになっている、 n行 6列の行列で表わしても良い。

以降、全ノヽンド速度ベクトルを改めて「ハンド速度ベクトル」と呼ぶ。また、任意の実数を要素とするハンド速度べクトルの集合を、改めて「全運動速度空間」と呼ぶ。この全運動速度空間は、 6 * n次元ベクトル空間になっている。

さらに、ある瞬間に実現不可能なハンド速度ベクトル全ての集合を、改めてこの瞬間の「拘束速度領域」と呼ぶ。 .ここでいう実現不可能なハンド速度べクトルは、幾何学的な観点から判定される。つまりこの瞬間に、ハンドと対象物の間またはハンドと他のハンドの間の干渉により、ほとんどハンドの変位を生じることなく応力が発生するハンド速度べクトルを実現不可能なハンド速度べクトルと定る。

このとき、腕ァクチユエータ能力や腕部リンク 5の関節配置などに関する制約は考えない。つまり、ハンド自身は、対象物がなければ任意の速度で移動可能であることを前提とする。また、ハンドおよび対象物が十分な剛性を備えていれば、剛体とみなす。従って、ある速度ベクトルが拘束速度領域の要素ベクトルならば、そのべクトルの全要素を k倍（kは任意の正の実数）したべクトルも、拘束速度領域の要素べクトルとする。尚、便宜上、零べクトルも拘束速度領域の要素ベタトノレとする。

そして、拘束速度領域の任意のベタトルとその逆べクトル（大きさが同じで向きが逆のベタトル）からなる集合を改めて「拘束速度空間」と定義する。通常は、拘束速度空間は、全運動速度空間の部分べクトル空間になっている。

また、拘束速度領域の任意のベクトルとの内積が零である（即ち、そのべクトルと直交する）全ての全運動速度空間内のベクトルの集合を、改めて「自由速度空間」と呼ぶ。

ここで、拘束速度空間おょぴ自由速度空間内のベクトルを、以下のように定義する。拘束速度空間の要素である方向べクトノレを、改めて「拘束方向べクトル」と呼ぴ、単に拘束方向というときは、任意の拘束方向ベクトルの向きを指す。また、自由速度空間の要素である方向ベクトルを、改めて「自由方向ベクトル」と呼び、単に自由方向というときは、任意の自由方向べクトルの向きを指す。

対象物およびハンドの運動の自由度と拘束に関する定義を、上記の如く拡張する。これにより、複数個（本）の腕を備えるロボットが複数個の腕を使って作業を行う場合でも、全ハンドを拘束方向に動かしてハンドに対象物から反作用（拘束力）を受けさせることで、全ての対象物の運動に影響を与えることなく、ロボットの動パランスを維持したり、傾いた姿勢を復元することができる。

以下に、複数個の腕による作業の例を示す。先ず、第 1 2図に示すように、対象物が、高さ h、前方 bに存在する Z軸まわりの回転だけができる、半径 rの円形なホイ一ノレ 1 3 0であって、このホイール 1 3 0を第 1ハンドと第 2ハンドの計 2個のハンドで把持して回す作業を例として挙げる。

ある瞬間の第 1ハンドの座標が（b, - r, h) であり、第 2ハンドの座標が (b, r , h) であるとする。この瞬間の自由速度空間は、任意の実数 ω z 1を持つハンド速度べクトノレ（r * ω z 1. 0， 0, 0， 0， ω ζ 1 , — rネ ω ζ ΐ , 0, 0, 0, 0, ω ζ ΐ) の集合である。これは、対象物の軸まわりに 2個のハンドを同じ角度だけ回転させる運動を意味する。

この瞬間の拘束速度空間は、任意の実数 Vx 1, Vy 1, Vz l， ω x 1 , ω y 1, ω ζ 1, Vx 2, Vy 2, V ζ 2, ω χ 2, ω y 2を持つハンド速度べクトル（Vx l， Vy 1 , V z 1 , ω 1 , ω y 1 , ω ζ ΐ, Vx 2, Vy 2, V ζ 2, ω χ 2, ω y 2, 一 r *Vx l— o z l + r *Vx 2) の集合である。例えば、ハンド速度ベクトル（Vx l, 0, 0, 0， 0， 0, Vx 1, 0, 0, 0 , 0, 0) は、拘束速度空間の速度ベクトルである。これは、ホイール 1 30の軸の摩擦が零であるならば、両ハンドを X方向に同じ力で押しても、ホイール 1

30の回転運動に影響を与えないことを意味する。

別の例を第 1 3図を参照して説明する。同図に示すように、第 1ハンドで固定物（例えば柱） 1 40を把持しながら、第 2ハンドでヒンジ 142の付いたドア 144を開閉する作業を例に挙げる。

ヒンジ 142の軸（142 Ζ) 方向が Ζ軸と平行であり、ヒンジ軸 14 2 Ζの X座標が b、ヒンジ軸の Y座標が]:、この瞬間の第 2ハンドの座標を（b， 0， h) とする。第 1ハンドの座標を（b， - c, h) とすると、自由速度空間は、任意の実数 ω z 2を持つハンド速度ベクトル（0, 0， 0， 0， 0， 0, r * co z 2, 0， 0， 0, 0, ω z 2) の集合である。

また、拘束速度空間は、それに直交するベクトル集合なので、任意の実数 Vx 1, Vy丄, V z l， ω 1 , ω y 1 , ω ζ 1 , "V χ 2 , V y 2 , V ζ 2 , ω χ 2, ω y 2を持つ、ノヽンド速度べクトノレ（Vx l, Vy 1 , V z 1 , ω 1 , ω y 1 , ω ζ ΐ, Vx 2, Vy 2, V ζ 2, ω χ 2, ω y 2, - r * Vx 2) の集合となる。

例えば、ノヽンド速度ベクトル（Vx 1 , Vy 1 , V z 1 , ω 1 , ω y 1 , ω ζ ΐ, 0, 0， 0， 0, 0， 0) は、拘束速度空間の速度ベクトルである。これは、第 1ハンドで固定物 1 4 0をどのように押しても、ドア 1 4 4の回転運動に影響を与えないことを意味する。

上記のように、複数のハンドから対象物に対して拘束方向に力を作用させても、対象物にはハンド以外の支持するものからその力を打ち消す拘束力し力発生せず、よって自由方向に摩擦力などを発生しないならば、ハンドおよび対象物の運動は変化しない。

従って、上記のように定義を拡張することにより、複数のハンドによる作業においても、 1個のハンドの場合と同じ原理で、対象物の運動制御に干渉せずに、ロボットの姿勢を安定にすることができる。即ち、複数のハンドによる作業においても、対象物からハンドに作用する力の拘束方向成分を制御することにより、対象物の運動に影響を与えることなく、ロボットの動バランスを維持したり、傾

V、た姿勢を復元することができる。

尚、一般的に、ハンドが複数、例えばこの実施の形態のように 2個になると、拘束速度空間の次元がハンドが 1個の場合よりも高くなるので、この中からロボットの姿勢安定化に効果の高い部分ベクトル空間を選び出し、その要素ベクトルの方向に拘束力を発生させることにより、ハンドが 1個の場合よりも高い効果を得ることができる。

ドアの開閉作業の例でいえば、ドアの開閉制御に干渉しないようにロボットの姿勢を復元させるためには、 1個の腕の場合ではドアに上下方向の力を作用させるのが最も効果的であるが、 2個の腕の例では、ドアを把持していないハンドで固定物を押すようにすることで、より効果的に動バランスを維持したり、傾いた姿勢を復元することができる。

以上の原理を前提とし、第 5図に示す姿勢安定ィヒメィン制御装置 1 0 6の説明に戻る。

姿勢安定化メィン制御装置 1 0 6は、前記した 3つの手法、即ち、

1 ) 実全床反力モーメントを修正する

2 ) 目標慣性力モーメントと目標重力モーメントを修正する

3 ) 実対象物反力の成分の内の拘束方向成分を修正する

の 3つの手法を併用するため、第 1 4図に示すように構成される。即ち、姿勢安定化メイン制御装置 1 o ⁶は、姿勢安定化用補償モーメント算出器 2 0◦、対象物反力平衡制御装置 2 0 2、対象物反力モーメント偏差分離器 2 0 4、修正目標対象物反力算出器 2 0 6、および各種の分配器（具体的には、対象物反力平衡制御用分配器 2 0 8と捕償モーメント分配器 2 1 0 ) とから構成される。

姿勢安定ィ匕メイン制御装置の各構成要素は全て、制御周期毎 (例えば 1 0 0 m s e c毎）に演算が行われる。 E C U 6 2の性能上、同時に複数の演算処理ができない場合には、第 1 4図の矢印上流側から順に演算処理を実行すれば良い。以下、姿勢安定化メィン制御装置 1 0 6で処理される変数およびそれらの構成要素について説明（定義）する。

上述したように、この発明に係る姿勢制御装置は、対象物反力の拘束方向成分を制御することにより、対象物の運動に影響を与えることなく、ロボット 1の動パランスを維持したり、傾いた姿勢を復元する制御装置に関する。しかしながら

、対象物反力の全ての拘束方向成分が、ロボット 1の動バランスを維持したり傾いた姿勢を復元する効果を持っているとは限らない。

また、ある拘束方向の目標対象物反力が、作業目的や対象物の性質上、変更すべきではない場合もある。つまり、対象物反力の拘束方向成分の内で、ロボット 1の動バランスを維持したり、傾いた姿勢を復元するために利用すべき成分を、復元する効果の度合、作業目的および対象物の性質を考慮して適宜選定すべきである。

そこで、新たに、次の空間と方向を定義する。第 2 4図を参照して説明すると、先ず、全運動速度空間の中で、ロボット 1の動バランスを維持したり、傾いた姿勢を復元するために利用すべき全ての拘束方向によって張られるべクトル空間を「操作空間」と呼ぶ。操作空間は、拘束速度空間の部分べクトル空間となっている。また、操作空間の任意の方向べクトルを「対象物反力操作方向」あるいは単に「操作方向」と呼ぶ。図示の如く、操作空間は、拘束速度空間から作業空間を除いた、白地の部分に相当する。

全運動速度空間の方向べクトルの内、任意の対象物反力操作方向との内積が零である（即ち、その方向と直交する）ものを「対象物反力非操作方向」あるいは単に「非操作方向」と呼び、全ての非操作方向が張る空間を「非操作空間」と呼ぶ。

即ち、非操作空間は、全運動速度空間の部分ベクトル空間となっており、自由速度空間は、非操作空間の部分ベクトル空間となっている。また、操作空間の任意のベタトルと非操作空間の任意のベタトルは直交する。

尚、操作方向は、コンプライアンス制御のコンプヺィアンス定数などと同じく、制御パラメータの一種と考えられる。この値は、一般には一定ではなく、作業が進行するにつれて変化する性質のものである。この値をどのように決定すべきかも重要なことであるが、この発明に係る姿勢制御装置の本質ではないので、この実施の形態では説明の便宜上、操作方向はあらかじめ作業目的、作業パターンおよび対象物の性質に基づいて決定されているものとする（操作方向などは作業中に決定しても良い）。

以上を前提として姿勢安定化メイン制御装置 1 0 6の上記した構成要素について詳説する。

姿勢安定化用補償モーメント算出器 2 0 0は、ロボット 1の姿勢傾斜偏差、より具体的には実上体傾斜と目標上体傾斜の差である上体傾斜偏差を零に収束させるための姿勢安定化用補償モーメント（実全床反力モーメントの修正に使用するモーメント）を算出する装置であり、上記手法 1 ) を達成するための構成である。この装置は、少なくとも、傾斜センサ 6 0によって検出された上体傾斜角度と目標作業パターン生成器 1 0 0が生成した目標上体傾斜角度の偏差に応じ、その偏差を減少するように姿勢安定化用補償モーメントを算出する。

第 1 5図に、姿勢安定化用捕償モーメント算出器 2 0 0を最も単純な構成で示す。図示の例では、入出力共に 1次元のスカラ量のように示されているが、実際には、 X方向成分と Y方向成分があり、それぞれに対し、図示の処理が行われる。但し、 Z方向成分は、姿勢安定性に関係がないので無視することとする。

以下、説明すると、先ず実上体傾斜と目標上体傾斜の差である上体傾斜偏差を求める。次に、姿勢安定ィヒ制御則部 2 0 0 aにおいて上体傾斜偏差の微分値を求め、最後に、式 1に従って姿勢安定ィ匕用補償モーメントを算出する。姿勢安定ィ匕用補償モーメント =一 k p *上体傾斜偏差

一 k d *上体傾斜偏差の微分値 ' · ·式 1 ここで、 k pと k dは制御ゲインである。尚、これ以外にも、 Hインフィュティ制御などを用いても良い。

第 1 4図に戻って説明を続けると、対象物反力平衡制御装置 2 0 2は、検出はできるが予期できない対象物反力を受けたとき、その対象物反力を考慮しながら目標上体位置姿勢を修正することにより、目標作業パターン生成器 1 0 0で生成された目標慣性力モーメントと目標重力モーメントを修正して動バランスを維持する制御を行なう装置であり、上記手法 2 ) を達成させるための構成である。また対象物反力モーメント偏差分離器 2 0 4は、対象物反力を姿勢安定化に利用する拘束方向の成分とそれ以外の成分に分離する装置であり、修正目標対象物反力算出器 2 0 6は対象物反力に姿勢安定化に必要な目標拘束力を加えた目標対象物反力を算出する装置である。

これら 2つの装置 2 0 4， 2 0 6に各種分配器（対象物反力平衡制御用分配器 2 0 8と補償モーメント分配器 2 1 0 ) を加えたものが、上記手法 3 ) を達成するための構成である。尚、それら装置の詳細は後述する。

先ず、対象物反力モーメント偏差分離器 2 0 4を説明すると、ハンド 4 O R ( L ) の力センサ 5 8で検出される実ハンド力は、ハンド基準点に作用する力と力のモーメントを、ハンド内にローカルに設定された座標系で表現したものとする。ここで、ハンド基準点は、ハンド 4 O R ( L ) の位置を表わすためのハンド内の基準点である。また、ハンド内にローカルに設定された前記座標系の原点は、ノヽンド基準点に設定する。

ところで、ハンド位置姿勢は、ハンド内にローカルに設定された前記座標系を支持脚座標系から見たときの原点位置と座標系の向きを表す。より具体的には、前記原点位置はべクトルで、前記座標系の向きは 3行 3列マトリックスで表現される。あるいは、ロボット工学で良く用いられる表現である、前記原点位置と前記座標系の向きをまとめて表現する同次行列を用いても良い。

また、ハンド 4 O R ( L ) の実際の位置姿勢は、前記した腕メイン制御装置 1 04によってァクチユエータ変位指令を介して駆動されることにより、ほぼ最終修正目標ハンド位置姿勢にあると考えられるので、ハンド内にローカルに設定された前記座標系は、最終修正目標ハンド位置姿勢を基準とした座標系であると考えても良い。

第 16図は対象物反力モーメント偏差分離器 204の構成を示すブロック図である。同図を参照して説明すると、対象物反力モーメント偏差分離器 204は実対象物反力モーメント成分分離器 204 aを備え、その分離器 204 aは支持脚座標系から見た最終修正目標ハンド姿勢を入力し、実ノヽンドカを支持脚座標系の値に変換する。支持脚座標系に変換された実ハンド力も、ハンド基準点に作用する力と力のモーメントで表現される。尚、支持脚座標系に変換された実ハンド力の作用点を全床反力中心点に変換したものが、実対象物反力である。即ち、これらは表現は異なる力同一のものを意味している。

ドが n個ある場合、支持脚座標系に変換された実ノヽンドカは、各ハンドのド力の各成分を順番に並べた 1つのベクトルで表現される。即ち、全運動速度空間でのベクトルで表現される。より具体的には、

(F i x, F l y, F 1 z , Mi x, Ml y , Ml z, F 2 x, F 2 y , F 2 z , M2 x, M2 y , M2 z , · · ·， Mn z)

の形で表現される。伹し、ここで、 F j X, F j y, F j _zは第 jハンドの基準点に作用する力の x, y， z成分、 Mj x， Mj y, M j zは第 jハンドの基準点に作用する力のモーメントの x, y, z成分を表わす。

次に、分離器 204 aは、支持脚座標系に変換した実ハンド力を、あらかじめ選定された操作方向に従って操作方向成分と非操作方向成分に分離する。尚、ここで注意すべき点は、各成分は、前述したように全運動速度空間のベクトルであつて、現実の 3次元空間のベタトルではないことである。

具体的には、分離器 204 aは、次のような演算によって分離する。先ず、操作空間が m次元であったとすると、操作空間の直交基底べクトルを A 1， A 2 , • · · , Amとする。また、非操作空間の直交基底ベクトルを B 1， B2， · ■ ·， B eとする。ここで、 mと eの和はハンドの個数の 6倍である。

次いで、支持脚座標系に変換された実ハンド力のベクトルを、 Fと記述する。また、実ハンド力の操作方向成分を F a、実ハンド力の非操作方向成分を F bと記述する。これらのベタトルは全運動速度空間のベタトルである。

実ハンド力の操作方向成分 F aと実ハンド力の非操作方向成分 F bは、式 2によって求められる。

F a = (Α 1 + Α 2 + · · · + Am) * F

F b = (Β 1 + Β 2 + · · - + B e ) * F · · '式 2 次に、分離器 2 0 4 aは、最終修正目標ハンド位置姿勢おょぴ実ハンド力の操作方向成分 F aに基づき、実ハンド力の操作方向成分 F aの全ての成分が、目標全床反力中心点まわりに作用する力の和を求める。これを「目標全床反力中心点まわりの実対象物反力操作方向成分和」と呼ぶ。この力は、 3次元空間での力と力のモーメントで表現される。座標系としては支持脚座標系を用いる。また目標全床反力中心点まわりの実対象物反力操作方向成分の内のモーメント成分を「目標^^反力中心点まわりの実対象物反力モーメント操作方向成分和」と呼ぶ。次に、分離器 2 0 4 aは、最終修正目標ハンド位置姿勢および実ハンド力の非操作方向成分 F bに基づき、実ハンド力の非操作方向成分 F bの全ての成分が、目標全床反力中心点に作用する力の和を求める。これを「目標全床反力中心点まわりの実対象物反力非操作方向成分和」と呼ぶ。この力も、 3次元空間での力と力のモーメントで表現される。座標系としては支持脚座標系を用いる。また目標全床反力中心点まわりの実対象物反力非操作方向成分の内のモーメント成分を「目標全床反力中心点まわりの実対象物反力モーメント非操作方向成分和」と呼ぶ。

また、対象物反力モーメント偏差分離器 2 0 4は目標ハンド位置作用点変換器 2 0 4 bを備え、その変換器 2 0 4 bも、上記のような処理（演算）を目標対象物反力に対しても行う。目標対象物反力も、実ハンド力と同様、全運動速度空間のべクトルである。具体的には先ず、目標対象物反力は目標全床反力中心点を作用点として表現されているので、変 » 2 0 4 bは、この作用点を一旦、目標ハンド位置（目標ハンドの基準点位置）を作用点とする力と力のモーメントに変換する。これを「目標ハンド位置を作用点とする目標対象物反力」と呼ぶ。

さらに、対象物反力モーメント偏差分離器 2 0 4は目標対象物反力モーメント成分分離器 2 0 4 cを備え、分離器 2 0 4 cは、上記と同様の処理によって操作方向成分と非操作方向成分に分離する。これらをそれぞれ「目標ハンド位置を作用点とする目標対象物反力の操作方向成分」および「目標ハンド位置を作用点とする目標対象物反力の非操作方向成分」と呼ぶ。

次に、分離器 2 0 4 cは、目標ハンド位置を作用点とする目標対象物反力の操作方向成分および目標ハンド位置（目標ハンドの基準点位置）に基づき、目標対象物反力の操作方向成分の全ての成分が、目標全床反力中心点に作用する力の和を求める。これを「目標全床反力中心点まわりの目標対象物反力操作方向成分和」と呼ぶ。この力は、 3次元空間での力と力のモーメントで表現される。座標系としては支持脚座標系を用いる。また、目標全床反力中心点まわりの目標対象物反力操作方向成分の内のモーメント成分を「目標全床反力中心点まわりの目標対象物反力モーメント操作方向成分和」と呼ぶ。

次に、分離器 2 0 4 cは、目標ハンド位置を作用点とする目標対象物反力の非操作方向成分おょぴ目標ハンド位置（目標ハンドの基準点位置）に基づき、目標対象物反力の非操作方向成分の全ての成分が、目標全床反力中心点に作用する力の和を求める。これを「目標全床反力中心点まわりの目標対象物反力非操作方向成分和」と呼ぶ。この力も、 3次元空間での力と力のモーメントで表現される。座標系としては支持脚座標系を用いる。また、目標全床反力中心点まわりの目標対象物反力非操作方向成分の内のモーメント成分を「目標全床反力中心点まわりの目標対象物反力モーメント非操作方向成分和」と呼ぶ。

次いで、加算点 2 0 4 dにおいて、力べして得られた目標全床反力中心点まわりの実対象物反力モーメント操作方向成分和から目標全床反力中心点まわりの目標対象物反力モーメント操作方向成分和を減じることにより、目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和を得る。

また、加算点 2 0 4 eにおいて、目標全床反力中心点まわりの実対象物反カモーメント非操作方向成分和から目標全床反力中心点まわりの目標対象物反力モーメント非操作方向成分和を減じることにより、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和を得る。

尚、目標全床反力中心点まわりの対象物反力偏差の内のモーメント成分を除く成分は、ロボット 1の姿勢にあまり関与しないので、この実施の形態では無視することとした。また、最終修正目標ハンド位置姿勢を用いる代り、脚部リンク 2 およぴ腕部リンク 5の実関節角を用いてキネマテイクス演算によって算出される実ハンド位置姿勢を用いても良い。

第 1 4図の説明に戻ると、上記のようにして得た目標^ *反力中心点まわりの対象物反力偏差モーメント操作方向成分和は、後に述べるように補償全床反カモ一メントの算出に使用される。他方、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和は、対象物反力平衡制御用分配器 2 0 8に入力される。

この対象物反力平衡制御用分配器 2 0 8を第 1 7図を参照して説明すると、分配器 2 0 8は関数 2 0 8 a , 2 0 8 bを備え、対象物反力平衡制御用分配器 2 0 8は、それら関数 2 0 8 a , 2 0 8 bに目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和を入力し、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和と、対象物反力平衡制御以外で平衡をとるべき対象物反力偏差モーメント和を出力させる。

尚、対象物反力平衡制御とは、前述の手法 2 ) を達成するために対象物反力平衡制御装置 2 0 2で行なわれる制御であり、概説すると、前記したように、上体位置をずらすことによって重力モーメントを発生させ、よって対象物反力偏差モーメント和を打ち消す制御である。

また、第 1 7図に示す例では、入出力共に 1次元のスカラ量のように示されているが、実際には、 X方向成分と Y方向成分があり、それぞれに対して図示の処理が行われる。但し、 Z方向成分は、姿勢安定性に関係がないので無視することとする。

図示の如く、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和を出力する関数 2 0 8 aとしては、上下限リミッタ特性（飽和特性）を持った関数を用いると共に、対象物反力平衡制御以外で平衡をとるべき対象物反力偏差モーメント和を出力する関数 2 0 8 bとしては不感帯特性を持った関数を用いるこのような関数特性を持たせた場合、動バランスを維持するために、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和の絶対値がある設定値以下のとき、そのモーメントを対象物反力平衡制御によって打ち消し、そのモーメントが設定値を越えたとき、超えた分を主にハンドの拘束方向に対象物反力を発生させて打ち消すように制御系が働く。即ち、前記上下限リミッタ特性を持たすことにより、上体位置のずらし量に制限を持たせ、幾何学的に無理な姿勢になるのを防止することができる。

尚、入出力関係を決定する 2つの関数 2 0 8 a , 2 0 8 bには、図示例以外の関数を用いても良い。例えば、第 1 8図に 2 0 8 c , 2 0 8 dで示すような曲線でも良い。第 1 7図おょぴ第 1 8図のいずれにしても、ロボット 1の特性、対象物の特性および作業内容に応じ、関数を選定すべきである。

また、入出力関係を決定する 2つの関数の出力和は、基本的には入力に一致させるべきであるが、厳密に一致させる必要はない。特に、脚コンプライアンス制御や腕コンプライアンス制御は、目標値の絶対値が大きいと、発生する力が目標に対して小さめになりやすい傾向があるので、これを補正する意味で、入力の絶対値が大きい領域では、関数出力の和の絶対値を入力の絶対値より大きくした方が良い場合もある。また、 X方向成分と Y方向成分を独立して分配するのでなく、 X方向成分と Y方向成分の両方を入力とした 2入力関数を用いて、各出力を決定しても良い。

第 1 4図の説明に戻ると、対象物反力平衡制御用分配器 2 0 8でかくして分配された、対象物反力平衡制御以外で平後 Ϊをとるべき対象物反力偏差モーメント和は、さらに乗算点 2 1 4で一 1倍されて極性が反転させられ、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントとして補償モーメント分配器 2 1 0に入力される。また、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和は、対象物反力平衡制御装置 2 0 2に入力される。

次いで、先に概説した対象物反力平衡制御装置 2 0 2について詳細に説明する先ずその機能について概説すると、対象物反力平衡制御装置 2 0 2は、上述したように目標上体位置姿勢などを入力し、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和に動力学的に平衡するように目標上体位置姿勢と目標全床反力を修正し、修正目標上体位置姿勢と対象物反力平衡制御用補償全床反力モーメントを出力する。

即ち、装置 2 0 2は、長期的には、目標上体位置姿勢をずらすことによって発生する重力モーメントが、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和に動力学的に平衡し、目標全床反力の修正量が零に戻る特徴を持つ前述の如く、対象物反力平衡制御装置 2 0 2は上記手法 2 ) を解決するための構成であり、本出願人が既に特開平 1 0— 2 3 0 4 8 5号公報で提案している技術であるが、この実施の形態においては、かかる公報に記載される対象物反力平衡制御装置の後半部の構成のみが使用されると共に、「対象物反力偏差」が入力されていた部分に、前記した「対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和」が入力されるように変更された点で異なる。

第 1 9図は対象物反力平衡制御装置 2 0 2の構成を示すプロック図であり、最終到達目標重心位置摂動量算出部 2 0 2 a、モデル制御則演算器 2 0 2 b , および摂動動力学モデル 2 0 2 cを備える。 '

これらへの入力は、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和、目標上体位置姿勢、最終修正目標ハンド位置姿勢、最終修正目標上体位置姿勢、最終修正目標足部位置姿勢である。尚、この内、最終修正目標ハンド位置姿勢、最終修正目標上体位置姿勢および最終修正目標足部位置姿勢は、対象物反力平衡制御において近似演算を用いる場合、即ち、摂動動力学モデル 2 0 2 cが近似モデルである場合には不要である。

尚、前述の姿勢安定化用補償モーメントは、部分的にも対象物反力平衡制御装置 2 0 2には入力されない。姿勢安定ィ匕用補償モーメントは、姿勢が傾いた結果ずれた重心位置を復元させるためにロボットに外部から与えられるべきモーメントであるが、対象物反力平衡制御装置 2 0 2の機能は、目標姿勢の重心をずらすことによって定常的な対象物反力偏差を打ち消すことであり、姿勢が傾いた結果ずれた重心を復元することはできないためである。

以下詳説すると、入力された対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和は、最終到達目標重心位置摂動量算出部 2 0 2 aに入力されるこの対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和を長期的に打ち消してパランスをとるための重心摂動量を「最終到達目標重心位置摂動量」と呼ぶ。最終到達目標重心位置摂動量算出部 2 0 2 aは、上記和から最終到達目標重心位置摂動量を算出して出力する。

最終到達目標重心位置摂動量は、モデル制御則演算器 2 0 2 bに入力される。モデル制御則演算器 2 0 2 bについて説明すると、最終到達目標重心位置摂動量と、摂動動力学モデル 2 0 2 cが出力する目標重心位置摂動量との差を重心変位偏差とすると、モデル制御則演算器 2 0 2 bは、この重心変位偏差を零に収束させるためのモーメントである対象物反力平衡制御用補償全床反力モーメントを P D制御則によって決定して出力する。

尚、摂動動力学モデル 2 0 2 cは、目標作業パターンの運動（摂動）に、ある拘束条件を与えておいた場合の、モデルのための目標全床反力モーメント摂動量と上体位置姿勢摂動量との関係を表すモデルである。

モデル制御則演算器 2 0 2 bの出力直後の加算点 2 0 2 dについて説明すると、加算点 2 0 2 dによって、摂動動力学モデルには、対象物反力平衡制御用補償全床反力モーメントと、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和が加算されたものが、モデルのための目標全床反力モーメント摂動量 (モデル入力量）として入力され、その入力に対応するように前記摂動動力学モデルによって上体位置姿勢摂動量が算出される。それが加算点 2 0 2 dで目標上体位置姿勢に加算され、修正目標上体位置姿勢が出力される。

第 1 4図の説明に戻ると、このように、対象物反力平後 ϊ制御装置 2 0 2の出力は、入力した目標上体位置姿勢を修正した修正目標上体位置姿勢と、対象物反力平衡制御用補償全床反力モーメントである。出力された対象物反力平衡制御用補償全床反力モーメントは、同図に示すように、続いて述べる補償モーメント分配器 2 1 0に入力される。

尚、上記で、装置 2 0 2から対象物反力平衡制御用捕償全床反力の内のモーメント成分のみが出力されるようにしたのは、対象物反力平衡制御用補償全床反力の内、ロボットの姿勢安定化のための特に重要な成分は、 X軸まわりモーメント成分と Y軸まわりモーメント成分であるためである。モーメント成分以外も考慮した制御系にした方がより良いが、顕著な効果は期待できない。

次いで、補償モーメント分配器 2 1 0について説明する。補償モーメント分配器 2 1 0は、第 2 0図に示すように、対象物反力平衡制御用補償全床反力モーメント、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントおょぴ姿勢安定ィ匕用補償モーメントを入力し、それらを目標拘束力モーメントと補償全床反力主モーメントに分配する、 2つの関数を備える。

ここで、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントは、前記したように、対象物反力平衡制御以外の手段で平衡をとるべき対象物反力偏差モーメント和を乗算点 2 1 4で一 1倍した値である。

また、目標拘束力モーメントは、前記入力の内で、拘束方向の対象物反力によつて目標全床反力中心点まわりに発生させるべきモーメントであり、捕償全床反力主モーメントは、前記入力の内で、目標全床反力中心点まわりに全床反力によつて発生させるべきモーメントである。

以下、第 2 0図を参照して分配処理について説明する。尚、図示の例では入出力共に 1次元のスカラ量のように示されている力実際には X方向成分と Υ方向成分があり、それぞれに対して処理が行われる。但し、 ζ方向成分は姿勢安定性に関係がないので無視することとする。

分配器 2 1 0は加算点 2 1 0 aを備え、加算点 2 1 0 aで対象物反力平衡制御用補償全床反力モーメントと、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントの和を求めて 2つの関数に入力する。ここで、姿勢安定化用補償モーメントを _u、前記和を Vと置き、前記した 2つの関数の中の 1つである関数 g l ( u , v ) の値を求め、これを補償全床反力主モーメントとする。また、他方の関数 g 2 ( u , V ) の値を求め、これを目標拘束力モーメントとする。但し、それぞれの関数は 2入力 1出力の関数である。

この関数は、例えば以下に示す式のようにすれば良い。尚、式中の C m a xおよび C m i nは設定値である。 u + V > Cm a Xのとき g 1 (u , v) = Cm a x

u+ v<Cm i nのとき

g 1 (u, v) =Cm i n

Cm i n≤u + v≤Cma xのとき

g l (u, v) =u + v · · ·式 3 g 2 (u , v) = u + v— g 1 (u, v) · · ·式 4 第 20図では、式 3、式 4で表わされる関数を、入力 uと入力 Vを水平面上の直交軸とし、出力値を高さで表している。

基本的には、補償モーメント分配器の入力和（対象物反力平衡制御用捕償全床反力モーメントと対象物反力平衡制御以外の手段に要求される対象物反力補償モ一メントと、姿勢安定化用捕償モーメントの和）力出力和（目標拘束力モーメントと補償全床反力主モーメントの和）に一致するように、 2つの関数を選定すべきである。即ち、 g l (u, v) +g 2 (u, v) =u + vが恒等的に成立するように、 2つの関数を選定すべきである。

尚、対象物反力平衡制御用補償全床反力モーメントを u、対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントを V、姿勢安定化用補償モーメントを wと置き、補償全床反力主モーメントと目標拘束力モーメントを、それぞれ 3入力の関数 g 1 (u, v, w) 、関数 g 2 (u, v, w) の値によって決定しても良い。

その場合でも、基本的には、 g l (u, v， ) + g 2 (u, v， w) =u + v+wが恒等的に成立するように関数を選定すべきである。し力しながら、複合コンプライアンス制御などの制御偏差を考慮し、出力和を大きめにしても良い。特に、姿勢安定化用補償モーメントはフィードバック量であるから、姿勢安定ィ匕用補償モーメントに影響を受けた出力成分の和が、姿勢安定化用補償モーメントと数十パーセント違っていてもあまり問題にならな!/、。

第 14図の説明に戻ると、補償モーメント分配器 210から出力される、一方のモーメントである目標拘束力モーメントは、加算点 215に送られ、そこで前述の目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和から減算される。

また、捕償モーメント分配器 2 1 0から出力される、他方のモーメントである補償全床反力主モーメントから加算点 2 1 6で、前述の目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和と目標拘束力モーメントの差が減算され、補償全床反力モーメントが出力される。

但し、腕ァクチユエータ 9 2のコンプライアンス制御の能力が高く、目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和がほぼ目標拘束力モーメントに一致するように制御される場合、加算点 2 1 6で目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和と目標拘束力モーメントの差を減じることなく、補償全床反力主モーメントをそのまま補償全床反力モーメントとしても良い。

尚、加算点 2 1 6を削除し、補償全床反力モーメントを補償全床反力主モーメントとし、その代わりに、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和から、目標全床反力中心点まわりの対象物反力偏差モーメント操作方向成分和と目標拘束力モーメントの差（加算点 2 1 5の出力）を減じるようにしても良い。

前記した目標拘束力モーメントは、修正目標対象物反力算出器 2 0 6にも入力される。

第 2 1図に、修正目標対象物反力算出器 2 0 6の構成を示す。同図に示すように、修正目標対象物反力算出器 2 0 6は、目標ハンド位置姿勢（あるいは最終修正目標ハンド位置姿勢）、目標拘束力モーメント、目標全床反力中心点位置、操作方向および目標対象物反力を入力し、それら入力に基づいて修正目標対象物反力を算出して出力する。

以下、その処理について説明すると、算出器 2 0 6は目標ハンド力修正量決定器 2 0 6 a、変換器 2 0 6 bおよび加算点 2 0 6 cを備える。目標ハンドカ修正量決定器 2 0 6 aは、目標ハンド位置（あるいは最終修正目標ハンド位置）を作用点とするように表現さ.れた目標対象物反力の修正量である目標ハンドカ修正量を以下のように決定する。即ち、決定器 206 aは、目標ハンド力修正量が目標ハンド位置（あるいは最終修正目標ハンド位置）に作用したときに、目標全床反力中心点に作用するモーメントの和と目標拘束力モーメントの差が零に近づくように、目標ハンドカ修正量を決定する。但し、決定器 206 aは、目標ハンド力修正量が、操作方向の力と力のモーメントであって、非操作方向成分を含まないように決定する。尚、目標ハンドカ修正量は、支持脚座標系で表現される。

最も簡単な決定法を以下に示す。先ず、操作方向の内のある 2つのベクトルを自由に選択する。各ベクトルは、操作空間の直交基底ベクトル A 1， A 2, · · ·， Amを適当に線形結合させることによって得られる。このベクトルをそれぞれ Ql, Q2とする。

次いで目標ハンド力修正量を、 aQl + bQ2の形とする。ここで、 a, bは係数である。

目標ハンドカ修正量 a Q 1 + b Q 2が目標ハンド位置（あるいは最終修正目標ハンド位置）に作用したとき、目標全床反力中心点に作用する合力のモーメント Mを、力学演算により求める。

求められた Mの X成分を Mx、 Y成分を Myとすると、 Mx, Myと a, の関係は、次の連立 1次方程式の形になる。ここで、 k l l, k 12, k 21およぴ k 22は係数である。 Mx =a k l l+b k l 2

My =a k 21+b k 22 · · ·式 5 次に、 Mxが目標拘束力モーメントの X成分、 Myが目標拘束力モーメントの Y成分に一致するように、 aおよび bを決定する。具体的には、式 5の連立方程式を aおよび bに関して解き、 Mxに目標拘束力モーメントの X成分の値、 My に目標拘束力モーメントの Y成分の値を代入することにより、 aおよび bを得る次に、前記ベクトル Ql， Q2および前記 a, bを用いて目標ハンド力修正量 aQl + bQ2の値を得る。得られた修正量は変 «206 に入力され、変換器 2 0 6 bは、目標ハンド力修正量を、作用点を目標ハンド位置（あるいは最終修正目標ハンド位置）力ら全床反力中心点に変更した表現に変換することにより、全床反力中心点を作用点とする目標対象物反力修正量を出力する。

最後に、加算点 2 0 6 cで、全床反力中心点を作用点とする目標対象物反力に、全床反力中心点を作用点とする目標対象物反力修正量を加えることにより、全床反力中心点を作用点とする修正目標対象物反力が出力される。尚、修正目標対象物反力は、全運動速度空間でのべクトルである。

第 1 4図の説明に戻ると、以上のようにして得た修正目標対象物反力およぴ前記した修正目標上体位置姿勢ならびに補償全床反力モーメントが、姿勢安定化メイン制御装置 1 0 6の最終的な出力となる。そして、第 5図に示すように、修正目標対象物反力が腕メイン制御装置 1 0 4に、補償全床反力モーメントが脚メイン制御装置 1 0 2に、修正目標上体位置姿勢がその両方に入力される。

脚メィン制御装置 1 0 2と腕メイン制御装置 1 0 4の機能を再説すると、脚メイン制御装置 1 0 2では、前述のように、目標全床反力中心点位置に作用する実全床反力のモーメント成分が、目標全床反力モーメント（通常は零）と補償全床反力モーメントの和に一致するように目標足部位置姿勢を修正し、さらに、修正目標上体位置姿勢と修正された目標足部位置姿勢から決定される目標脚関節変位に実関節変位が追従するように、ァクチユエータ変位指令を出力して脚関節ァクチユエータを制御する。

また、腕メイン制御装置 1 0 4では、前述のように、ハンド 4 O R ( L) の力センサ 5 8によって検出される実対象物反力（実ハンド力）と修正目標対象物反力の差に応じ、差が零に近づくように、 P D制御などによって目標ハンド位置姿勢を修正する。さらに、修正目標上体位置姿勢（あるいは最終修正目標上体位置姿勢）と修正された目標ハンド位置姿勢から決定される目標腕関節変位に実関節変位が追従するように、ァクチユエータ変位指令を出力して腕関節ァクチユエ一タを制御する。

以上のように、姿勢安定化メィン制御装置 1 0 6は、目標全床反力中心点に作用する対象物反力偏差モーメントを、操作方向成分と非操作方向成分とに分離し、前記非操作方向成分によって発生する動力学的アンバランスを、その一部を対象物反力平衡制御装置によって打ち消し、残りを、目標全床反力中心点に作用すべき目標対象物反力モーメント操作方向成分を修正することにより、言い換えれば、腕の動作を修正して前記操作方向成分を変化させることにより打ち消すように構成した。

換言すれば、先の提案技術において、対象物から受ける予期せぬ反力が急激に変化したとき、対象物反力平衡制御装置 2 0 2によつて行なわれていた、足部 2 2 R ( L) (および脚部リンク 2 ) を動かして踏み止めさせる（踏ん張らせる）動作を、腕部リンク 5の動作によって軽減、あるいはなくすことができる。このため、全床反力モーメントが限界を越えて発生する恐れがなく、よってロボット 1の動パランスを維持することができ、口ポットが傾いたり、転倒したりすることを防止できる。

また、操作方向成分が変化するように腕の動作を修正することから、ロボット

1の動バランスを維持したり、傾いた姿勢を復元しても、対象物の運動に影響を与えることがない。

最後に、上記した制御系の安定性について説明する。

目標全床反力中心点に作用すべき目標対象物反力モーメント操作方向成分の修正量である目標拘束力モーメントと、目標全床反力中心点まわりの対象物反力偏差モーメントの操作方向成分和と非操作方向成分和の関係は、前記した第 1 4図に示すようになる。

理解の便宜のため、姿勢安定ィヒメィン制御装置 1 0 6において対象物反力モーメント偏差分離器 2 0 4が分離する、目標全床反力中心点に作用する対象物反力偏差モーメントの操作方向成分和と非操作方向成分和の内、非操作方向成分和によつて発生する動力学的ァンバランスを全て目標全床反力中心点に作用すべき目標対象物反力モーメント操作方向成分を修正することによって打ち消すように構成したと仮定する。即ち、対象物反力平衡制御を働力、せない場合を想定する。その場合、腕制御系（第 5図に示す腕メイン制御装置 1 0 4ゃァクチユエータ 9 2、ァクチユエータ駆動装置 8 8などからなる制御系）と姿勢安定ィ匕メイン制御装置 1◦ 6からなる制御系は、対象物反力に注目すると、第 2 2図のように簡略ィ匕することができる。尚、第 2 2図における外乱モーメントは、対象物に想定していた性質と異なる性質を持つていたため、対象物が予期せぬ挙動を示したことによって発生する対象物反力、即ち、想定外の対象物反力である。例えば、対象物に発生する摩擦が、想定していたものと異なっていたことによって発生する。外乱モーメントの中の操作方向成分を「外乱モーメント操作方向成分」、非操作方向成分を「外乱モーメント非操作方向成分」と呼ぶ。

第 2 2図で加算点 2 2 0で入力される外乱モーメント非操作方向成分が変化すると、これを打ち消すように目標拘束力モーメントが決定され、加算点 2 2 2で加算されることで、目標全床反力中心点まわりの目標対象物反力モーメント操作方向成分和が修正され、操作方向腕コンプライアンス制御部で目標ハンド位置姿勢が修正量だけ修正され、さらに腕部リンク 5のァクチユエータ変位指令が修正され、ロボット 1の腕部リンク 5が駆動されて動バランスが保持される。また、外乱モーメント操作方向成分が変化しても、腕のコンプライアンス制御への目標値は変化しない。

他方、姿勢安定化メイン制御装置 1 0 6が、前記非操作方向成分和によって発生する動力学的アンバランスを、目標全床反力中心点に作用すべき目標対象物反力モーメント非操作方向成分和を修正することによって打ち消すように構成したと仮定すると、腕制御系と姿勢安定化メイン制御装置 1 0 6からなる制御系は、対象物反力に注目すると、第 2 3図のように簡略ィ匕することができる。

この場合、姿勢安定化メイン制御装置を含むフィードパックループは正帰還となり、発散する。

即ち、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和から姿勢安定化メィン制御装置によって目標拘束力モーメントが算出され、それが加算点 2 2 6で入力（目標全床反力中心点まわりの目標対象物反力モーメント非操作方向成分和）に加算されて目標全床反力中心点まわりの修正目標対象物反力モーメント非操作方向成分和が算出され、それが加算点 2 2 8で目標全床反力中心点まわりの実対象物反力モーメント非操作方向成分和から減算され、目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和が算出されるまでのフィードバックループは、正帰還となり、発散する。

即ち、第 2 3図に示す構成においては、ひとたび外乱モーメント非操作方向成分が発生すると、これを打ち消そうと目標全床反力中心点まわりの修正目標対象物反力モーメント非操作方向成分和が限りなく増加あるいは減少して発散することとなる。

それに対し、この実施の形態に係る移動ロポットの姿勢制御装置にあっては、第 1 4図などに示す如く、目標全床反力中心点まわりの対象物反力モーメントを操作方向成分和と非操作方向成分和に分離し、つ、第 2 2図に示す如く、正帰還にならないようにフィードパックループを構成することにより、このような発散を防ぐようにした。即ち、非操作方向側でフィ一ドバックされた値が操作方向側に加算されるように構成することで、そのような発散を防ぐようにした。

第 2 5図は、この発明の第 2の実施の形態に係る移動ロボットの姿勢制御装置である。

第 1の実施の形態と相違する点に焦点をおいて説明すると、第 2の実施の形態においては、対象物反力平衡制御用分配器 2 0 8の出力の中、対象物反力平衡制御で平衡をとるべき対象物反力偏差モーメント和を零（同図に破線で示す）とすると共に、分配器 2 0 8の出力を全てそれ以外の手段で平衡をとるべき対象物反力偏差モーメント和とした。

尚、残余の構成および効果は第 1の実施の形態のそれと異ならない。第 2の実施の形態に係る移動ロボットの姿勢制御装置は上記の如く構成したので、第 1の実施の形態に係る装置と同様の効果を得ることができる。

第 2 6図は、この発明の第 3の実施の形態に係る移動ロボットの姿勢制御装置である。

第 1の実施の形態と相違する点に焦点をおいて説明すると、第 3の実施の形態においては、 2足の脚式移動ロボット 1に代え、図示のような車輪式の移動ロボットについて姿勢制御を行うようにした。

以下、説明すると、図示の車輪式の移動ロボット 3 0 0は、円筒形の基体（上体） 3 0 2と、それに上下（Z軸方向）にストロークするアクティブサスペンシヨン（図示せず）を介して取りつけられる 4個の車輪（図で 2個のみ示す） 3 0 4を備える。基体 3 0 2の上部にはマウント 3 0 6が回転自在に取りつけられると共に、その上に 1個の屈曲自在なアーム（腕部リンク） 3 1 0が取りつけられる。

即ち、アーム 310は、マウント 306に取りつけられる第 1リンク 310 a と、第 1リンク 310 aに関節（図示せず）を介して取りつけられる第 2リンク 310 bと、第 2リンク 310 bの自由端側に関節（図示せず）を介して取りつけられるハンド 3 10 cからなる。関節は、それぞれ電動モータなどのァクチュエータを内蔵する。

基体 302の内部には、マイクロコンピュータからなる電子制御ユニット（E CU) 312が格納される。基体 302の重心位置の付近には傾斜センサ（図示せず）が配置され、 Z軸に対する基体 302の傾きとその角速度に応じた出力を生じる。車輪 304のそれぞれには加重センサ（図示せず）が配置され、接地面から 4個の車輪 304にそれぞれ作用する床反力（加重） F l， F 2, . . を検出する。

ここで、マウント 306の Z軸まわりの回転角度を Θ 1、第 1リンク 310 a の Z軸に対する角度を Θ 2、第 1リンク 310 aに対する第 2リンク 310 の相対角度を 03、第 2リンク 31 O bに対するハンド 310 cの相対角度を 04 とすると、移動ロボット 300の目標動作は、 0 1から 04の動作パターンと Z MPで表すことができる。

また、図示の移動ロボット 300の姿勢制御において、第 1リンク 310 aの Z軸に対する角度 02を制御するものとすると、 Θ 2が第 1の実施の形態の脚式移動ロボット 1の目標上体位置に、 0 1, Θ 3, S 4の組が目標上体姿勢に相当する。また、 04カヽンド 310 cの位置姿勢に相当する。アクティブサスペンシヨンを制御する電子制御ユニット（ECU) 312の動作が第 1の実施の形態の第 8図に示す構成の中の複合コンプライアンス動作決定部の動作に相当し、加重 Fを F 1から F 4に分配して制御する。

従って、第 3の実施の形態において車輪式の移動ロボット 300について上記のような制御を行うとき、第 1の実施の形態で述べた脚式移動ロボット 1の姿勢制御と同様の効果を得ることができる。

以上のように、第 1から第 3の実施の形態に係る移動ロボットの姿勢制御装置にあっては、少なくとも基体（上体 3) と、移動機構（脚部リンク 2あるいは車輪 3 0 4 ) と、対象物に力を作用することができる、少なくとも 1個の腕部リンク 5 (あるいはアーム 3 1 0 ) からなる移動ロボット 1， 3 0 0の姿勢制御装置において、想定外の外力が作用するとき、前記想定外の外力の、ある所定方向の成分である第 1の外力（自標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和）に応じ、前記所定方向に直交する方向に前記腕部リンクに第 2の外力（対象物反力平衡制御以外の手段に要求される対象物反力補償モーメント）が作用するように、換言すれば、前記目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和に基づいて得られる対象物反力平衡制御以外の手段で平渙 Ϊをとるべき対象物反力偏差モーメント和の極性を反転して得られる、即ち、それに直交する対象物反力平衡制御以外の手段に要求される対象物反力補償モーメント和の極性を反転して得られる、即ち、それに直交する対象物反力平衡制御以外の手段に要求される対象物反力補償モーメントが作用するように、前記腕部リンクを駆動して、換言すれば目標値を修正することを必要とせず、前記口ポットの姿勢を安定させるように制御する如く構成した。

また、少なくとも基体（上体 3 ) と、移動機構（脚部リンク 2あるいは車輪 3 0 4 ) と、対象物に力を作用することができる、少なくとも 1個の腕部リンク 5 (あるいはアーム 3 1 0 ) からなる移動ロボット 1 , 3 0 0の姿勢制御装置におレ、て、少なくとも前記ロボットの目標腕部リンク位置姿勢（目標ハンド位置姿勢 ) と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作（目標対象物反力あるいは目標位置姿勢）を生成する目標動作生成手段（目標作業パターン生成器 1 0 0 ) 、前記対象物反力の実際値である実対象物反力（実ハンド力）を検出あるいは（オブザーバを介して）推定する実対象物反力検出手段（検出については力センサ 5 8 , 荷重センサ、 E C U 6 2 , 3 1 2、推定についてはオブザーバ）、少なくとも前記検出あるいは推定された実対象物反力に基づき、前記ロボットの姿勢が安定するように前記目標動作を修正する姿勢安定化制御手段（姿勢安定化メイン制御装置 1 0 6 ) 、およぴ少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置（ァクチユエータ 9 0， 9 2 ) を備える如く構成した。

また、前記姿勢安定化制御手段は、少なくとも前記実対象物反力と前記目標対象物反力との差を示す対象物反力偏差（目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和）に基づき、前記口ポットの姿勢が安定するように前記目標動作を修正する如く構成した。

また、前記姿勢安定化制御手段は、前記対象物反力偏差の第 1の成分（目標全床反力中心点まわりの対象物反力偏差モーメント非操作方向成分和）によって発生する転倒力の全部あるいは一部を、前記第 1の成分から得られる第 2の成分（対象物反力平衡制御以外の手段に要求される対象物反力補償モーメント）によって打ち消すように、前記目標動作を修正する如く構成した。

また、少なくとも基体（上体 3 ) と、移動機構（脚部リンク 2あるいは車輪 3 0 4 ) と、対象物に力を作用することができる、少なくとも 1個の腕部リンク 5 (あるいはアーム 3 1 0 ) からなる移動ロポット 1， 3 0 0の姿勢制御装置において、少なくとも前記ロボットの目標腕部リンク位置姿勢（目標ハンド位置姿勢 ) と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作（目標対象物反力あるいは目標位置姿勢）を生成する目標動作生成手段（目標作業パターン生成器 1 0 0 ) 、前記ロボットの姿勢傾き偏差（上体傾斜角度）を検出し、前記検出した姿勢傾き偏差、より具体的には検出値に基づいて算出される姿勢安定ィヒ用補償モーメント）が零に近づくように前記目標動作を修正する姿勢安定ィヒ制御手段（姿勢安定ィヒメイン制御装置 1 0 6

) 、および少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置（ァクチユエータ 9 0， 9 2 ) を備える如く構成した。

尚、上記において、目標全床反力中心点まわりの実対象物反力非操作方向成分和は、実対象物反力の全ての成分が、目標全床反力中心点まわりに作用する力の和から、目標全床反力中心点まわりの実対象物反力操作方向成分和を減じることによって求めても良い。なぜなら、目標全床反力中心点まわりの実対象物反力操作方向成分和と目標全床反力中心点まわりの実対象物反力非操作方向成分和の和は、実対象物反力の全ての成分が、目標全床反力中心点まわりに作用する力の和に等しいからである。

逆に、目標全床反力中心点まわりの実対象物反力操作方向成分和は、実対象物反力の全ての成分が、目標全床反力中心点まわりに作用する力の和から、目標全床反力中心点まわりの実対象物反力非操作方向成分和を減じることによって求めても良い。

さらには、目標全床反力中心点まわりの対象物反力操作方向成分和と、目標全床反力中心点まわりの対象物反力非操作方向成分和に関しても同様の方法で求めても良い。

また、姿勢安定化メイン制御装置 1 0 6において、目標対象物反力を修正する代わりに目標ハンド位置姿勢を変更しても良い。より詳細に説明すると、腕メイン制御装置 1 0 4は、通常、対象物反力とハンド位置姿勢を目標値に一致させるように制御することはできないので、対象物反力の制御とハンド位置姿勢の制御にトレードオフが働き、目標ハンド位置姿勢を変更するだけでも実対象物反力が変化する。

従って、このときの目標ハンド位置姿勢の変更量と実対象物反力の変化の比（即ち腕メイン制御系のコンプライアンス定数）が分かっていれば、上述の実施の形態で求められた目標対象物反力の修正量に、その比を乗じることにより、目標ハンド位置姿勢の修正量を求めることができるためである。

また、脚式移動ロボットとして 2個の脚部リンクと 2個の腕部リンクを備えたヒユーマノィドロボットを例にとって説明したが、 2個以外の個数の脚部リンクを備えた脚式移動ロボットロボットでも良く、腕部リンクの個数も 1個あるいは 3個以上でも良い。

また、脚部リンクであっても、床以外の対象物に作用するものであれば、腕部リンクとみなして良い。例えば、昆虫型の 6個の脚部リンクを備えたロボットにおいて、頭部側の前の 2個の脚部リンクを用いて物を持ち上げる場合、その脚部リンクを腕部リンクとみなすことができる。

また、ロボットの動力学的アンパランスを X軸方向と Y軸方向に対する傾きとして説明したが、遊脚の振りなどに起因する Z軸まわりのスピンに対しても同様に適用することができる。

また、上体リンクの曲げやひねりのためのァクチユエータを設けなかったが、それを追加するとき、上体ァクチユエータ制御装置も必要となる。但し、上体リンクの曲げやひねりは、腕または脚の付け根側に関節を追カ卩したことと等価であるので、概念上、腕または脚のァクチユエータとみなすことができる。即ち、上体ァクチユエータ制御装置は、腕または脚の制御装置の一部として含まれると考えることができる。

また、腕部リンクの制御に、仮想コンプライアンス制御装置以外の別の手段、例えば、電動ァクチユエータを電流指令型のアンプによって制御する手段を用いて関節トルクを制御し、その結果、間接的に対象物反力を制御するようにしても良い。その制御にはハンドに力センサを配置する必要はないが、対象物反力制御装置用にやはり力センサを設けるのが望まし!/、。

また、ハンド 4 O R ( L) の力センサ 5 8の代わりに、関節トルクから実対象物反力を推定する推定器を腕メイン制御装置に備えても良い。この推定器は、従来技術である外乱オブザーバーを用いれば良い。

また、目標対象物反力を零に設定しても良い場合もある。例えば、前記した作業 4では手摺りとハンドの間の摩擦がほぼ零であれば、目標対象物反力を零に設定するのが良い。また、作業 2で台車の自在キャスタ輪の摩擦が不明ならば、目標対象物反力を零に設定すれば良い。尚、目標対象物反力が常に零である場合では、対象物反力偏差は実対象物反力と一致するので、上記において対象物反力偏差に代えて実対象物反力を用いても良い。

また、床が平面でない場合でも、本出願人が特開平 5— 3 1 8 3 4 0号公報で提案した仮想平面を想定する技術を用い、目標全床反力中心点や目標 Z M Pを仮想平面上に求めることで、適用が可能となる。

また、上記において、ブロック図は演算処理順序を変えるなど、種々の変形が可能である。産業上の利用可能性

この発明によれば、移動ロボットの姿勢制御装置において、想定外の外力が作用するとき、想定外の外力の、ある所定方向の成分である第 1の外力に応じ、所定方向に直交する方向に腕部リンクに第 2の外力が作用するように、腕部リンクを駆動して前記ロボットの姿勢を安定させるように制御する如く構成したので、移動ロボットが対象物から反力をうけるような作業を行っているとき、姿勢が不安定になったり、あるいは対象物から予期せぬ反力を受けても、動パランスを維持して安定な姿勢を保持することができる。

Claims

請求の範囲

1 . 少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも 1個の腕部リンクからなる移動ロボットの姿勢制御装置において、想定外の外力が作用するとき、前記想定外の外力の、ある所定方向の成分である第 1の外力に応じ、前記所定方向に直交する方向に前記腕部リンクに第 2の外力が作用するように、前記腕部リンクを駆動して前記ロボットの姿勢を安定させるように制御することを特徴とする移動ロボットの姿勢制御装置。

2 , 少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも 1個の腕部リンクからなる移動ロポットの姿勢制御装置において、 a . 少なくとも前記ロボットの目標腕部リンク位置姿勢と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成する目標動作生成手段、

b . 前記対象物反力の実際値である実対象物反力を検出あるいは推定する実対象物反力検出手段、

c 少なくとも前記検出あるいは推定された実対象物反力に基づき、前記ロボットの姿勢が安定するように前記目標動作を修正する姿勢安定化制御手段、および

d . 少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置、

を備えたことを特徴とする移動ロボットの姿勢制御装置。

3 . 前記姿勢安定化制御手段は、少なくとも前記実対象物反力と前記目標対象物反力との差を示す対象物反力偏差に基づき、前記ロボットの姿勢が安定するように前記目標動作を修正することを特徴とする請求の範囲第 2項記載の移動口ポットの姿勢制御装置。

4 . 前記姿勢安定化制御手段は、前記対象物反力偏差の第 1の成分によって発生する転倒力の全部あるいは一部を、前記第 1の成分から得られる第 2の成分によつて打ち消すように、前記目標動作を修正することを特徴とする請求の範囲第 3 項記載の移動ロボットの姿勢制御装置。

5 . 少なくとも基体と、移動機構と、対象物に力を作用することができる、少なくとも 1個の腕部リンクからなる移動ロボットの姿勢制御装置において、 a . 少なくとも前記ロボットの目標腕部リンク位置姿勢と、前記対象物から前記腕部リンクに作用する対象物反力の目標値である目標対象物反力からなる目標動作を生成する目標動作生成手段、

b . 前記口ポットの姿勢傾き偏差を検出し、前記検出した姿勢傾き偏差が零に近づくように前記目標動作を修正する姿勢安定化制御手段、

および

c 少なくとも前記修正された目標動作に基づいて前記腕部リンクを駆動する腕部駆動装置、

を備えたことを特徴とする移動ロボットの姿勢制御装置。