WO1998026905A1 - Controleur d'attitude de robot mobile sur jambes - Google Patents

Description

明細書

脚式移動 Πボッ 卜の姿勢制御装置

技術分野

この発明は、 脚式移動ロボッ トの姿勢制御装置に関し、 より詳しくは脚式移動 ロボッ 卜、 特に 2足歩行の脚式移動ロボッ 卜において予期せぬ対象物反力を受け ても動的バランスをとつて姿勢の安定性を保つことができるようにしたものに関 し、 特に腕を備える脚式移動ロボッ 卜の腕と脚の脚腕協調制御としての応用が有 効である。

尚、 この明細書で 『対象物反力』 は作業対象を含む環境から受ける外力で、 口 ボッ トに接地面から作用する床反力を除いたものを指称する意味で使用する。 背景技術 '

脚式移動ロボッ ト、 特に 2足歩行の脚式移動ロボッ 卜で腕を備えたものとして は、 「上体運動により 3軸モ一メントを補償する 2足歩行ロボッ トの開発」 （日 本ロボッ ト学会誌 1 1巻第 4号、 1 9 9 3年 5月） が知られている。 このロボッ トは単純化された腕としての振り子を備え、 これを振ることによって発生する重 力と慣性力も含めて目標歩容を予め設計しておき、 それに追従するように歩行制 御する。

しかしながら、 この従来技術においては腕は対象物反力を受けないことが前提 となっている。 従って、 そこで提案されている制御を歩行だけではなく、 作業に も適用したとき、 作業対象から予期せぬ反作用を受けると動バランスを崩し、 姿 勢が不安定になつたり、 最悪の場合には転倒する恐れがあつた。

また、 本出願人も特開平 7— 2 0 5 0 6 9号公報で同種の脚式移動ロボッ トを 提案しており、 そこにおいては歩行時に摩擦力が低下したとき腕を振らせて安定 な姿勢を回復するようにしている。

しかしながら、 本出願人が提案した脚式移動ロボッ トにおいては、 脚と腕が協 調せずに独立して別々に制御されているため、 腕を駆動すると、 腕が発生する重 力と慣性力、 および作業対象からの反作用によってロボッ ト全体の動バランスが 崩れ、 却ってロボッ 卜の姿勢が不安定になる場合があった。 従って、 この発明の目的は上記した不都合を解消することにあり、 脚式移動口 ボッ トが予期できない対象物反力を受けても、 動バランスをとつて安定な姿勢を 継続できるようにした脚式移動ロボッ トの姿勢制御装置を提供することにある。

この発明の第 2の目的は、 対象物反力が急変するときも、 それに静的にバラン スする位置にロボッ トの重心を移動させることにより、 傾きや転倒を効果的に抑 制するようにした脚式移動ロボッ トの姿勢制御装置を提供することにある。

この発明の第 3の目的は、 上記した対象物反力を受けたとき、 ロボッ 卜の重心 が移動する過渡期においても、 重心位置や床反力を適正に変化させて動バランス を維持し続けられるようにした脚式移動ロボッ トの姿勢制御装置を提供すること に る。

この発明の第 4の目的は、 脚式移動ロボッ 卜で腕を備えるものにおいて、 予め 想定していなかつた動作パターンで腕を動かして作業をするときに腕に発生する 重力、 慣性力だけでなく、 作業対象から予期せぬ反作用を受けても、 動バランス をとつて安定な姿勢を継続できるようにした脚式移動ロボッ トの姿勢制御装置を 提供することにある。 発明の開示

上記の目的を達成するために、 この発明にあっては、 少なくとも基体と、 前記 基体に連結される複数本のリンクからなる脚式移動ロボッ トの姿勢制御装置にお いて、 前記ロボッ トの少なくとも前記基体の目標軌跡を含む運動パターンと、 前 記ロボッ 卜に作用する床反力の目標軌跡と、 前記ロボッ 卜に作用する床反力以外 の外力の目標軌跡とを少なくとも含む、 前記ロボッ トの目標歩容を設定する目標 歩容設定手段、 前記床反力以外の外力を検出する外力検出手段、 前記検出された 外力と、 前記目標軌跡で設定された床反力以外の外力の偏差を演算する外力偏差 演算手段、 前記床反力の摂動と前記ロボッ トの重心位置および/または基体の位 置の摂動の関係を表現するモデル、 少なくとも前記演算された外力の偏差に基づ いて前記モデルに入力すべきモデル入力量を演算するモデル入力量演算手段、 前 記演算されたモデル入力量を前記モデルに入力し、 得られる前記重心位置および

Zまたは基体の摂動量に応じて前記基体の目標軌跡を修正する、 基体目標軌跡修 正量を演算する基体目標軌跡修正量演算手段、 少なくとも前記演算されたモデル 入力量に応じて前記床反力の目標軌跡を修正する、 床反力目標軌跡修正量を演算 する床反力目標軌跡修正量演算手段、 および、 少なく とも前記演算された基体目 標軌跡修正量および床反力目標軌跡修正量に基づいて前記ロボッ 卜の関節を変位 させる関節変位手段、 を備える如く構成した。

ここで 『位置』 は、 重心位置を除き、 『位置および Zまたは姿勢』 を含む意味 で使用する。 尚、 『姿勢』 は後述の如く 3次元空間における向きを意味する。 ここで、 『床反力の目標軌跡』 とは、 より具体的には、 少なくとも床反力中心 点の目標軌跡を含む意味で使用する。 また 『前記床反力の目標軌跡を修正する』 とはより具体的には、 床反力中心点まわりのモーメントを修正する意味で使用す る。

ここで、 『外力を検出する』 とは、 検出のみならず外乱オブザーバなどを使用 して推定することも含む意味で使用する。

更には、 前記モデル入力量演算手段は、 前記外力に静的に平衡する平衡重心位 置の摂動量を算出する平衡重心位置摂動量算出手段、 を備え、 前記算出された平 衡重心位置に前記モデルが収束するように前記モデル入力量を演算する如く構成 した。

更には、 前記モデルが前記ロボッ トを倒立振子で近似するモデルである如く構 成した。

更には、 前記平衡重心位置摂動量算出手段は、 前記算出された平衡重心位置の 摂動量を所定の範囲に制限するリ ミッタ、 を備える如く構成した。

更には、 前記床反力目標軌跡修正量演算手段は、 前記演算された床反力目標軌 跡修正量を所定の範囲に制限するリ ミッ夕、 を備える如く構成した。

更には、 前記床反力の目標軌跡は、 前記ロボッ トに作用する床反力の目標中心 点の軌跡を少なくとも含む如く構成した。

更には、 前記床反力目標軌跡修正量演算手段は、 前記床反力目標軌跡修正量が 、 前記モデル入力量から前記外力の偏差を減算した値と、 前記床反力の目標中心 点まわりに作用するモーメントに動力学的に釣り合うように、 前記床反力目標軌 跡修正量を演算する如く構成した。 更には、 前記床反力以外の外力が、 前記リ ンクを介して前記ロボッ トに作用す る作業対象物からの反力である如く構成した。

更には、 前記ロボッ トが、 前記基体に連結される 2本の脚リンクと 2本の腕リ ンクからなる脚式移動ロボッ トである如く構成した。

更には、 少なく とも基体と、 前記基体に連結される複数本のリ ンクからなる脚 式移動ロボッ トの姿勢制御装置において、 前記ロボッ トの少なく とも前記基体の 目標位置を含む運動パターンと、 前記ロボッ トに作用する床反力の目標中心点の 軌跡を少なく とも含む、 前記ロボッ トの目標歩容を設定する目標歩容設定手段、 前記リ ンクを介して前記ロボッ 卜に作用する、 作業対象物からの反力を検出する 対象物反力検出手段、 前記検出された対象物反力を前記目標床反力中心点まわり のモーメントとして変換する対象物反力モーメント変換手段、 前記変換された対 象物反力モ一メントに動力学的に釣り合うように、 前記目標中心点まわりの床反 力モーメ ン トと前記口ボッ トの位置および姿勢を修正するロボッ ト位置 ·姿勢修 正手段、 および前記修正された目標中心点まわりの床反力モーメントと前記ロボ ッ 卜の位置 ·姿勢に基づいて前記ロボッ トの関節を変位させる関節変位手段、 を 備える如く構成した。

更には、 少なく とも基体と、 前記基体に連結される複数本のリンクからなる脚 式移動ロボッ トの姿勢制御装置において、 前記ロボッ トの少なく とも前記基体の 目標位置を含む運動パターンを設定する目標歩容設定手段、 前記リ ンクを介して 前記ロボッ トに作用する、 作業対象物からの反力を検出する対象物反力検出手段 、 前記検出された対象物反力を所定の点まわりのモーメントとして変換する対象 物反力モーメ ント変換手段、 前記変換された対象物反力モーメン トに動力学的に つりあうように、 前記所定の点まわりの床反力モーメントと前記ロボッ 卜の位置 および姿勢を修正するロボッ ト位置，姿勢修正手段、 および前記修正された所定 の点まわりの床反力モーメントと前記ロボッ トの位置 ·姿勢に基づいて前記ロボ ッ トの関節を変位させる関節変位手段、 を備える如く構成した。

上記で、 『脚式移動ロボッ ト』 は腕以外に対象物反力を受ける脚式移動ロボッ トを含む。 また、 『腕リンク』 に関しては、 脚リンクであっても、 それが作業対 象物に作用するものであれば、 腕リンクとみなすものとする。 例えば、 昆虫型の 6脚ロボッ トにおいて、 前の 2脚を用いて物を持ち上げる場合には、 その脚リン クは腕リンクとみなすこととする。 図面の簡単な説明

第 1図は、 この発明に係る脚式移動ロボッ トの姿勢制御装置を全体的に示す説 明図である。

第 2図は、 第 1図に示す 2足歩行ロボッ トの制御ュニッ 卜の詳細を示すプロッ ク図である。

第 3図は、 この発明に係る脚式移動ロボッ トの姿勢制御装置の構成および動作 を機能的に示すプロック図である。

第 4図は、 第 1図に示す脚式移動ロボッ トが腕を使用して行う作業を示す説明 図である。

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

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

第 7図は、 第 3図に示す対象物反力平衡制御装置の動作を説明するタイミング •チャートである。

第 8図は、 第 3図に示す対象物反力平衡制御装置の詳細な構成を示すプロック 図の前半部である。

第 9図は、 第 3図に示す対象物反力平衡制御装置の詳細な構成を示すプロック 図の後半部である。

第 1 0図は、 第 9図に示す対象物反力平衡制御装置の摂動動力学モデルを示す 説明図である。

第 1 1図は、 第 1 0図に示すモデルを倒立振子で近似した状態を示す説明図で ある。

第 1 2図は第 9図に類似する、 この発明の第 2の実施の形態を示す対象物反力 平衡制御装置の詳細な構成を示すプロック図の後半部である。

第 1 3図は、 第 9図に類似する、 この発明の第 3の実施の形態を示す対象物反 力平衡制御装置の詳細な構成を示すプロック図の後半部である。

第 1 4図は、 第 1 1図に類似する、 この発明の第 4の実施の形態を示す倒立振 子型摂動動力学モデルを示す説明図である。 発明を実施するための最良の形態

以下、 添付図面を参照してこの発明に係る脚式移動ロボッ トの姿勢制御装置を 説明する。 尚、 脚式移動ロボッ トとしては 2足歩行ロボッ トを例にとる。

第 1図はその脚式移動ロボッ トの姿勢制御装置を全体的に示す概略図である。 図示の如く、 2足歩行ロボッ ト 1は左右それぞれの脚リンク 2に 6個の関節を 備える （理解の便宜のために各関節をそれを駆動する電動モ一夕で示す） 。

6個の関節は上から順に、 腰部の脚回旋用の関節 1 0 R， 1 0 L (右側を R、 左側をしとする。 以下同じ） 、 腰部のロール軸 （Y軸まわり） の関節 1 2 R， 1 2 L、 同ピッチ軸 （X軸まわり） の関節 1 4 R, 1 4 L、 膝部のロール軸の関節 1 6 R， 1 6 L、 足部のロール方向の関節 1 8 R, 1 8 L、 および同ピッチ軸の 関節 2 O R, 2 0 Lから構成される。 足部には足平 2 2 R, 22 Lが取着される 上記において股関節 （あるいは腰関節） は関節 1 O R (L) ， 1 2 R (L) , 1 4 R (L) から、 足関節は関節 1 8 R (L) ， 2 0 R (L) から構成される。 また股関節と膝関節とは大腿リンク 24 R, 24 L、 膝関節と足関節とは下腿リ ンク 2 6 R, 2 6 Lで連結される。

更に、 腰部の上位には上体 （あるいは基体。 リンクで示す） 2 8が設けられる と共に、 その上端には左右それぞれの 7個の関節からなる腕リンク 3を備える （ 同様に、 理解の便宜のために各関節をそれを駆動する電動モータで示す） 。

7個の関節は上から順に、 肩部のロール軸の関節 3 0 R, 3 0 L、 同ピッチ軸 の関節 3 2 R, 3 2 L、 腕の回旋用の関節 34 R， 34 L、 肘部のロール軸の関 節 3 6 R, 3 6 L、 手首回旋用の関節 3 8 R, 3 8 L、 同ロール軸の関節 4 0 R ， 4 0 L、 および同ピッチ軸の関節 4 2 R， 4 2 Lから構成される。 手首の先に はハンド （エンドエフェク夕） 4 4 R， 4 4 Lが取着される。

上記において肩関節は関節 3 0 R (L) ， 3 2 R (L) , 34 R (L) から、 手首関節は関節 3 8 R (L) , 4 O R (L) ， 4 2R (L) から構成される。 ま た肩関節と肘関節とは上腕リンク 4 6 R, 4 6 L、 肘関節と手首関節とは下腕リ ンク 4 8 R, 4 8 Lで連結される。

尚、 上体 （基体） 2 8の内部には、 第 2図に関して後述するマイクロコンピュ 一夕からなる制御ュニッ ト 5 0などが格納される。

上記の構成により、 脚リ ンク 2は左右の足についてそれぞれ 6つの自由度を与 えられ、 歩行中にこれらの 6 * 2= 1 2個の関節を適宜な角度で駆動することで 、 足全体に所望の動きを与えることができ、 任意に 3次元空間を歩行させること ができる （この明細書で 「*」 は乗算を示す） 。 尚、 前記の如く、 ロボッ 卜の進 行方向 （ピッチ軸） を X軸、 左右方向 （ロール軸） を Y軸、 鉛直方向 （重力軸） を Z軸とする。

また、 腕リ ンク 3は左右の腕についてそれぞれ 7つの自由度を与えられ、 これ らの 7 * 2 = 1 4個の関節を適宜な角度で駆動することで、 後述する台車を押す などの所望の作業を行うことができる。

第 1図に示す如く、 足関節の下方の足平 22 R (L) には公知の 6軸力センサ 5 6が取着され、 ロボッ トに作用する外力の内、 接地面から oボッ トに作用する 床反力の 3方向成分 F x, F y, F zとモーメントの 3方向成分 Mx， My, M zとを検出する。

更に、 手首関節とハンド 4 4 R (L) の間には同種の 6軸力センサ 5 8が取着 され、 ロボッ トに作用するそれ以外の外力、 特に作業対象物から受ける前記した 対象物反力の 3方向成分 F X, F y, F zとモーメントの 3方向成分 Mx， My ， Mzとを検出する。

また、 上体 2 8には傾斜センサ 6 0が設置され、 Z軸 （鉛直軸 （重力軸） ） に 対する傾きとその角速度を検出する。 また各関節の電動モー夕はその出力を減速 •増力する減速機 （図示せず） を介して前記したリンク 24， 2 6 R (L) など を相対変位させると共に、 その回転量を検出する口一夕リエンコーダが設けられ て変位検出器付の脚ァクチユエ一夕あるいは腕ァクチユエ一夕として構成される 。 これら 6軸力センサ 5 6などの出力は制御ュニッ ト 5 0に送られる （図示の便 宜のためロボッ ト 1の右側についてのみ図示する） 。 第 2図は制御ュニッ ト 5 0の詳細を示すプロック図であり、 マイクロ ' コンビ ユー夕から構成される。 そこにおいて傾斜センサ 6 0などの出力は A / D変換器

7 0でデジタル値に変換され、 その出力はバス 7 2を介して R A M 7 4に送られ る。 また各ァクチユエ一夕において電動モ一夕に隣接して配置されるエンコーダ の出力は、 カウン夕 7 6を介して R A M 7 4内に入力される。

制御ュニッ ト内には C P Uからなる演算装置 8 0が設けられており、 演算装置

8 0は後述の如く、 R O M 8 4に格納されている歩容に基づいてロボッ 卜が安定 な姿勢を継続することができるように、 関節角変位指令 （ァクチユエ一夕変位指 令） を算出し、 R A M 7 4に送出する。

また演算装置 8 0は R A M 7 4からその指合と検出された実測値とを読み出し 、 各関節の駆動に必要な制御値 （操作量） を算出して D ZA変換器 8 6 と各関節 に設けられたァクチユエ一夕駆動装置 （アンプ） 8 8を介して各関節を駆動する 脚ァクチユエ一夕と腕ァクチユエ一夕の電動モー夕に出力する。

第 3図は、 この発明に係る脚式移動ロボッ トの姿勢制御装置 （主として前記し た演算装置 8 0に相当） の構成および動作を機能的に示すブロック図である。 この装置は脚および腕の動作を統合的に制御する装置であり、 各ァクチユエ一 夕駆動装置 8 8に対する変位指令を出力する。 図示の如く、 この装置は、 目標作 業パターン生成器、 対象物反力平衡制御装置、 脚メイン制御装置、 および腕メイ ン制御装置から構成される。

以下に、 理解の便宜のため、 第 4図に示すロボッ ト作業状況を例に挙げて、 こ の装置の各構成要素の処理内容を説明する。 第 4図では、 ロボッ ト 1が台車 1 0 0を押しているとき、 台車から受ける実対象物反力の絶対値が目標作業パターン において想定していた目標対象物反力よりも突然小さくなってしまったため、 こ のずれによってロボッ ト 1はバランスを崩し、 前に傾きかけている状況とする。 この実施の形態に係る装置は、 このような状況においても常に動バランスを維持 するように制御するものである。

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

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

また、 環境から受ける反力を表現する変数は、 目標全床反力中心点 （位置） （ 目標 Z M P (位置） ) 、 目標全床反力および目標対象物反力から構成される。 ' これら各変数は、 支持脚座標系で表される。 支持脚座標系は、 支持脚足首 （関 節 1 8， 2 0 R ( L ) の交点） から足平 2 2 R ( L ) への垂直投影点を原点とす る座標系であり、 第 5図および第 6図に示すように、 支持脚が接触している床に 固定された座標系であり、 支持脚足平の前向きを X軸の向き、 左向きを Y軸の向 き、 鉛直方向上向きを Z軸向きとする座標系である。

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

前記の如く、 ロボッ トが環境から受ける外力の内で、 各足平床反力を除いた外 力を対象物反力と呼ぶとき、 目標対象物反力はその目標値である。 第 4図の例で はハンド 4 4 R ( L ) が対象物 1 0 0から受ける反力のことである。

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

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

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

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

歩行制御の分野において従来から公知である Z M Pの概念も、 概念を次のよう に拡張する。 即ち、 ロボッ トの運動によって生じる慣性力と重力と対象物反力の 合力が、 その点を作用点とする力とモーメ ン トで表現されたとき、 X軸まわりモ ーメ ント成分と Y軸まわりモーメント成分が 0になる床面上の点を、 Z M Pと呼 ぶ。 ロボッ トが目標の運動を行う時の Z M Pを目標 Z M P (位置） と呼ぶ。

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

目標作業パターン生成器では、 動力学的平衡条件を満足する目標作業パターン を生成する。 従って、 目標作業パターン生成器が生成する目標全床反力中心点 （ 位置） は目標 Z M P (位置） に一致する。

目標足平位置，姿勢、 目標上体位置 ·姿勢、 目標ハンド位置 ·姿勢は、 前記し た支持脚座標系で表現されたそれぞれの部位の位置と姿勢を表す。 具体的にはこ の明細書で、 上体 2 8の位置およびその速度は、 上体 2 8の重心位置などの代表 点およびその （変位） 速度を意味する。 更に、 上体あるいは足平の姿勢は、 X , Y , Z空間における 『向き』 を意味する。

対象物反力平衡制御装置およびその制御はこの実施の形態の制御の中心をなす もので、 対象物反力平衡制御装置は姿勢バランスをとるために動力学的平衡条件 を考慮しながら制御を行う。 そこで、 対象物反力平衡制御装置の概要を説明する 前に、 動力学的平衡条件について以下に説明する。

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

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

1 ) 慣性力モ一メ ント

2 ) 重力モ一メ ント

3 ) 全床反力モ一メント 4 ) 対象物反力モーメン ト

以上のモ一メ ントは先にも説明したが、 改めて以下に定義する。

慣性力モーメ ン トは、 目標全床反力中心点まわりのロボッ 卜の角運動量の変化 によって生じるモーメ ン トである。 この値はオイラー方程式によって求められ、 具体的には目標全床反力中心点まわりのロボッ トの角運動量の 1階微分値の符号 を反転させたものである。

目標作業パターンの慣性力モーメン トを、 目標慣性力モーメン トと呼ぶ。 実際 のロボッ トが作業しているときの慣性力モーメントを実慣性力モ一メントと呼ぶ ο

重力モーメ ン トは、 ロボッ トの重心に作用する重力が目標全床反力中心点まわ りに作用するモ一メ ン トである。

各足平に作用する床反力の合力を、 全床反力と呼ぶ。 全床反力モーメ ン トは、 全床反力が目標全床反力中心点まわりに作用するモーメントである。

作業対象物から受ける反力を、 対象物反力と呼ぶ。 対象物反力モーメ ン トは、 作業対象物反力が目標全床反力中心点まわりに作用するモーメントである。

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

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

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

ところが、 実際には、 実対象物反力モーメン トが目標対象物反力モーメントと 一致せず差が生じる。 例えば、 第 4図に関して述べたように、 台車を押す作業を 行っているときに台車 （すなわち目標対象物） の実際のころがり摩擦力の絶対値 が想定していた値よりも突然小さくなってしまった状況である。 この図の状況では、 実対象物反力が目標全床反力中心点の Y軸まわりに作用す るモ一メ ン トは、 目標対象物反力が目標全床反力中心点の Y軸まわりに作用する モーメ ン トよりも正の向きに大きくなって条件 1を満たさなくなり、 ロボッ ト 1 は前傾する。 尚、 モーメン トの向きは、 座標軸の正方向に向いてロボッ ト 1を時 計まわりに回転させるモ一メントを正とする。

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

手法 1 ) 上記偏差を打ち消すように、 実全床反力モーメン トを変える。 具体的に は、 目標全床反力中心点まわりに負の床反カモ一メ ントを発生するように脚メイ ン制御装置に指令し、 脚メイン制御装置において、 この指令を受けて、 足平 2 2 R ( L ) のつまさきを下げ、 実全床反力モーメン トを負の向きに増加させる。 即 ち、 足で踏ん張るような姿勢をとらせる。

手法 2 ) 上記偏差を打ち消すように、 目標作業パターンの運動パターンを修正す ることにより、 目標慣性力モーメン卜と目標重力モーメントを修正する。 具体的 には、 目標上体位置および または姿勢を修正することによって、 目標慣性カモ 一メ ン トと目標重力モーメン トを修正する。 即ち、 上体を前に移動させる。

この実施の形態に係る装置では両方の手法を同時に行い、 短期的には手法 1を 主に使うことによって速い変化に対応し、 長期的には手法 2を主に使うことによ つて実全床反力モーメントを元の目標全床反力モーメントに収束させながら、 常 に動バランスを維持するようにした。

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

実全床反力モーメントを元の目標全床反力モーメントに戻すためには、 重心位 置をずらし、 目標重力モーメン トによって上記偏差を打ち消すように、 手法 2に よって目標作業パターンの運動パターンを修正すれば良い。 但し、 重心位置を急 激にずらすと、 過大な目標慣性力モーメントが逆向きに発生するので、 ゆっくり と重心位置をずらす必要がある。 従って、 手法 2は長期的な対応に向いている。 上記を前提として対象物反力平衡制御装置について説明する。 対象物反力平衡 制御装置は、 上記の制御機能を持った装置である。

対象物反力平衡制御装置の入力は、 目標上体位置，姿勢、 目標全床反力中心点 (位置） 、 目標対象物反力、 6軸力センサ 5 8の検出値、 最終修正目標ハン ド位 置，姿勢、 最終修正目標上体位置 ·姿勢、 最終修正目標足平位置 ·姿勢である （ 尚、 近似演算を用いる場合は、 最終修正目標ハン ド位置，姿勢、 最終修正目標上 体位置 ·姿勢、 最終修正目標足平位置 ·姿勢は不要である） 。

対象物反力平衡制御装置では、 上記の制御機能を実現するために、 目標対象物 反力を実対象物反力の検出値に置き換え、 それに動力学的に平衡するように目標 上体位置 ·姿勢と目標全床反力を修正する。 これにより修正された作業パターン が想定している対象物反力 （即ち、 修正された目標対象物反力） と実対象物反力 がー致し、 ロボッ トの動力学的平衡条件が満足される。

対象物反力平衡制御装置の出力は、 修正目標上体位置 ·姿勢と対象物反力平衡 制御用補償全床反力である。

修正目標上体位置 ·姿勢は、 対象物反力平衡制御装置によって修正された目標 上体位置 ·姿勢である。 対象物反力平衡制御用補償全床反力は、 目標全床反力中 心点 （位置） に、 修正によって加えられる全床反力である。 尚、 対象物反力平衡 制御用補償全床反力の成分の内で、 姿勢安定化のための特に重要な成分は、 X軸 まわりモーメ ン ト成分と Y軸まわりモーメン ト成分である。

対象物反力平衡制御装置の出力の挙動だけを述べると、 実対象物反力と目標対 象物反力の偏差、 より正確には両者のモーメ ントの偏差が急変、 即ち、 第 7図に 示すようにステップ状に変化する場合には、 動力学平衡条件を満足するために、 最初は対象物反力平衡制御用の補償全床反力のモーメ ント成分が、 この差に応じ てすばやく応答する。

その後しばらくすると、 修正目標上体位置 '姿勢が、 この偏差に静的に釣り合 う位置 ·姿勢に整定し、 対象物反力平衡制御用補償全床反力のモーメ ント成分は 0に収束する。 尚、 対象物反力平衡制御装置の構成とアルゴリズム説明は後述す る。

第 3図において、 脚メイン制御装置に入力される目標値は、 修正目標上体位置 ，姿勢、 目標足平位置，姿勢、 目標全床反力中心点 （位置） とその点に作用する 目標全床反力と対象物反力平衡制御用補償全床反力である。

脚メイン制御装置の機能は、 簡単に言うならば、 脚のァクチユエ一夕 （関節 1 O R ( L ) などの電動モータおよびエンコーダ） を操作し、 目標姿勢に追従する 姿勢安定化制御と目標床反力に追従する床反力制御を同時に行う装置である。 尚 、 目標姿勢と目標床反力を同時に完全に満足させることは不可能であるので、 適 当な調整が行われ、 長期的には両方を満足するように制御される。

より詳しくは、 傾斜センサ 6 0によって検出された実上体位置 ·姿勢を修正目 標上体位置 ·姿勢に復元させるために、 目標全床反力中心点に発生させるべき復 元全床反力を算出し、 目標全床反力中心点に作用する実全床反力のモーメント成 分が、 この復元全床反力と目標全床反力と対象物反力平衡制御用補償全床反力の 合力のモ一メント成分に一致するように、 足平 2 2 R ( L ) を回転あるいは上下 動させるベく 目標足平位置 ·姿勢を修正する。 修正された目標足平位置 ·姿勢を 最終修正目標足平位置 ·姿勢と呼ぶ。

従って、 傾斜センサ 6 0によって検出される実上体位置 ·姿勢とその変化率が 、 修正目標上体位置，姿勢とその変化率に一致しているならば、 目標全床反力中 心点位置に作用する実全床反力のモーメント成分が目標全床反力と対象物反力平 衡制御用補償全床反力の合力のモーメント成分に一致するように目標足平位置 · 姿勢を修正する。

脚メイン制御装置は、 さらに、 修正目標上体位置 ·姿勢と修正目標足平位置 - 姿勢から決定される目標脚関節変位に実関節変位が追従するように脚ァクチユエ 一夕を制御する。

脚メイン制御系は脚メイン制御装置、 および前記した傾斜センサ 6 0、 足平 2 2 R ( L ) に設けた 6軸力センサ 5 6、 ァクチユエ一夕駆動装置 8 8およびァク チユエ一夕 （関節 1 0 R ( L ) ないし 2 0 R ( L ) 用電動モー夕およびェンコ一 ダ） から構成される。

脚メイン制御装置で修正された目標足平位置 ·姿勢は、 最終修正目標足平位置 ，姿勢として対象物反力平衡制御装置に送られる。 但し、 対象物反力平衡制御装 置において、 目標足平位置 ·姿勢が修正されたことによるロボッ 卜の重心位置の 変化が無視できるならば、 最終修正目標足平位置 ·姿勢を対象物反力平衡制御装 置に送る必要はない。

第 3図において、 腕メイン制御装置に入力される目標値は、 修正目標上体位置

•姿勢、 目標ハン ド位置 ·姿勢および目標対象物反力である。

腕メイン制御装置の機能は簡単に言うならば、 腕のァクチユエ一夕 （関節 3 0 R ( L ) などの電動モータ、 エンコーダ他） を操作して、 目標姿勢に追従する姿 勢制御と目標対象物反力に追従する対象物反力制御を同時に行うことである。 目 標姿勢と目標対象物反力を同時に完全に満足させることは不可能であるので、 適 宜な手法、 例えば、 従来からマニピュレータのコンプライアンス制御、 いわゆる 仮想コンプライアンス制御として知られるものを用いる （機械工学便覧、 ェンジ ニァリング編、 C 4— 1 0 0頁） 。

具体的な制御系構成とアルゴリズムを以下に説明すると、 腕メイン制御系は腕 メイン制御装置、 および前記したハン ド 4 4 ( L ) に設けた 6軸力センサ 5 8、 ァクチユエ一夕駆動装置 8 8および腕ァクチユエ一夕 （関節 3 O R ( L ) ないし 4 2 R ( L ) 用電動モー夕およびエンコーダ） から構成される。

腕メイン制御装置は、 6軸力センサ 5 8によって検出される実対象物反力と目 標対象物反力の差に応じて目標ハンド位置 ·姿勢を修正する。 修正された目標ハ ン ド位置 ·姿勢を、 最終修正目標ハン ド位置 '姿勢と呼ぶ。 腕メイン制御装置は 、 修正目標上体位置 ·姿勢と最終修正目標ハン ド位置 ·姿勢から決定される目標 腕関節変位に実関節変位が追従するように腕ァクチユエ一夕を制御する。

ここで、 対象物反力平衡制御装置の詳細を説明する。

第 8図は対象物反力平衡制御装置の制御構成図の前半部分を、 第 9図は対象物 反力平衡制御装置の制御構成図の後半部分を示す機能ブ αック図である。

第 8図を参照して前半部分の処理から説明する。

先ず、 実際のハンド 4 4 R ( L ) は、 腕メイン制御装置によって、 ほぼ、 最終 修正目標ハンド位置 ·姿勢にあると考えられるので、 6軸力センサ 5 8によって 検出された実対象物反力を、 修正目標ハンド位置 ·姿勢によって支持脚座標系の 原点まわりの力とモーメン卜に変換する （実関節変位からキネマティ クス演算に よって、 実ハン ド位置 ·姿勢を求めて、 これを用いて実対象物反力を変換しても 良い） 。

次に、 変換された実対象物反力を、 目標全床反力中心点まわりの力とモーメ ン 卜に変換することにより、 目標全床反力中心点まわりの実対象物反力モーメント を得る。 最後に、 これから目標全床反力中心点まわりの目標対象物反力モーメン トを引く ことにより、 目標全床反力中心点まわりの対象物反力モーメント偏差を 得る。

次に第 9図を参照して対象物反力平衡制御装置の後半部分の処理を説明する。 先ず、 そこで用いる摂動動力学モデルについて説明する。

摂動動力学モデルは、 目標作業パターンの運動 （摂動） にある拘束条件を与え ておいた場合の、 目標全床反力モーメント摂動量と上体位置 ·姿勢摂動量との関 係を表すモデルである。 以下に、 例として、 第 1 0図に示すようにロボッ トの上 体姿勢を目標上体姿勢に一致させたまま、 上体の水平位置を摂動するモデルを説 明する。

ここで、 以下のように記号をとりきめる。

m ： ロボッ ト全質量

g ：重力加速度

h ： 目標全床反力中心点からの重心高さ

△x G ： 目標重心位置摂動量の X成分

A y G ： 目標重心位置摂動量の Υ成分

△x b ： 目標上体位置摂動量の X成分

A y b ： 目標上体位置摂動量の Y成分

Δ Μ χ ： 目標全床反力中心点まわりの目標全床反力モーメン ト摂動量の X成分 △M y ： 目標全床反力中心点まわりの目標全床反力モーメン ト摂動量の Y成分 △M G x ： 目標全床反力中心点まわりの目標重力モーメン ト摂動量の X成分 △M G y ： 目標全床反力中心点まわりの目標重力モーメ ン ト摂動量の Y成分 厶 L x ： 目標全床反力中心点まわりの目標角運動量の摂動量の X成分

A L y ： 目標全床反力中心点まわりの目標角運動量の摂動量の Y成分 d(a) I dt：変数 aの時間微分

d( d(a) I dt) /dt：変数 aの時間 2階微分

重力モーメントの定義から、 次式が導かれる。

AMG X = - Δ y G *m g

AMGy=AxG*mg

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

AL x = - mh * d(Ay G) / dt

Δ L y =mh * dCA x G) I dt

• · ·式 2 オイラー方程式により、 次式が導かれる。 d(A L X) / dt = AUG x + ΔΜχ

d(AL y) / dt = AMGy + AUy

• · ·式 3 式 1、 式 2および式 3より、 摂動動力学モデルの運動方程式として、 次式を得 ることができる。 mh * d( ά(Δ x G) / dt ) / dt= 厶 xG*mg + ΔΜγ

mh * d( d(A y G) / dt ) / dt= Ay G*mg - ΔΜχ

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

Δ X b = k * Δ x G

Δ y b = k * Δ y G

■ · ·式 5 以上から、 摂動動力学モデルは、 式 4と式 5を用い、 目標重心位置摂動量と目 標上体位置摂動量を算出する。 詳しくはこの装置の如く、 デジタル演算の場合に は、 式 4は離散化して使用する。 ちなみに、 式 4は、 第 1 1図に示す、 高さ h、 質量 mの倒立振子の運動方程式に一致する。

第 9図に示す対象物反力平衡制御装置後半部において、 前記した目標全床反力 中心点まわりの対象物反力モーメ ント偏差は、 最終到達目標重心摂動量算出部に 入力される。

この目標全床反力中心点まわりの対象物反力モーメント偏差を長期的に打ち消 してバランスをとるための重心摂動量を最終到達目標重心位置摂動量と呼ぶ。 最 終到達目標重心位置摂動量算出部は、 上記偏差から最終到達目標重心位置摂動量 を算出する。

ここで、

Δ Μ ο X ：対象物反力モ一メント偏差の X成分

Δ Μ ο y ：対象物反力モーメント偏差の Y成分

△ M G 0 X ：最終到達目標重心位置摂動量によって発生する重力モーメントの X 成分

△ M G 0 y ：最終到達目標重心位置摂動量によって発生する重力モーメントの Y 成分

△ x G e：最終到達目標重心位置摂動量の X成分

△ y G e：最終到達目標重心位置摂動量の Y成分

とする。

対象物反力モーメ ン ト偏差を最終到達目標重心位置摂動量によつて発生する重 力モ一メントによって打ち消すためには、 次式を満足する必要がある。

AMG 0 X = -ΔΜ 0 X

AMG 0 y = - Δ M 0 y

• · ·式 6 最終到達目標重心位置摂動量によって発生する重力モーメントは、 次式のよう に o

△ MG o x = -m g * Δ y Ge

AMG o y = mg 氺 AxGe

' · ·式 7 式 6、 式 7より次式を得る。

Δ X Ge = - ΔΜ 0 y /m g

Δ y Ge = ΔΜ o x/m g

• · ·式 8 故に、 最終到達目標重心位置摂動量は、 式 8によって算出すれば良い。

対象物反力平衡制御装置のモデル制御則演算器について説明すると、 最終到達 目標重心位置摂動量と摂動動力学モデルが出力する目標重心位置摂動量との差を 、 重心変位偏差と呼ぶ。 モデル制御則演算器は、 この重心変位偏差を 0に収束さ せるための制御を行う。 出力は、 対象物反力平衡制御用補償全床反力モーメント である。

具体的には、 次式のような、 PD制御則によって、 対象物反力平衡制御用補償 全床反力モーメントを決定すれば良い。 対象物反力平衡制御用補償全床反力モーメ ン ト = K p *重心変位偏差

+ K d *重心変位偏差の微分値

. . ·式 1 0 ここで、 K pは比例ゲイン、 K dは微分ゲインである。

モデル制御則演算器の出力直後の加算点について説明すると、 モデル制御則演 算器の出力直後の加算点によって、 摂動動力学モデルには、 対象物反力平衡制御 用補償全床反力モーメン トと、 目標全床反力中心点まわりの対象物反力モ一メ ン ト偏差の和が、 モデルのための目標全床反力モーメ ン ト摂動量 （モデル入力量） として入力され、 その入力に対応する目標上体位置 ·姿勢摂動量が算出される。 これが目標上体位置 ·姿勢に加算され、 修正目標上体位置 ·姿勢が作られる。 ところで、 摂動動力学モデルは動力学的平衡条件を満たすので、 モデル出力で ある目標上体位置 ·姿勢摂動量によって発生する目標慣性力モーメント摂動量お よび目標重力モ一メント摂動量と、 モデル入力との和は 0である。

故に、 次式が成立する。 目標慣性力モーメ ン ト摂動量 十 目標重力モーメ ン ト摂動量

+ 対象物反力平衡制御用補償全床反力モーメン ト + 対象物反力モーメ ン ト偏差 = 0

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

故に、 次式が成立する。 実全床反力モーメント摂動: 対象物反力平衡制御用補償全床反力モーメ ン 卜

式 1 2 式 1 1 と式 1 2から、 次式が得られる。 目標慣性力モーメ ン ト摂動量 + 目標重力モーメン ト摂動量

+ 実全床反力モーメン ト摂動量 + 対象物反力モーメン ト偏差 二 0

. . '式 1 3 ところで、 目標作業パターンは動力学的平衡条件を満足しているから、 次式を 満足する。 目標慣性力モーメン ト + 目標重力モーメン ト

+ 実全床反力モーメン ト + 目標対象物反力モーメ ン ト = 0

• · .式 1 4 各修正モーメ ン 卜が元のモ一メン トにモ一メ ン ト摂動量を加えたものであるこ と、 実対象物反力モーメン卜が目標対象物反力モーメ ントと対象物反力モーメ ン ト偏差の和であること、 および式 1 3と式 1 4力、ら、 恒等式として次式が得られ る。 修正目標慣性力モーメン ト + 修正目標重力モーメン ト

+ 修正実全床反力モーメン ト + 実対象物反力モーメン ト = 0

. · ·式 1 5 式 1 5は、 実対象物反力モーメン トがいかに目標対象物反力モーメン トからず れようとも、 対象物反力平衡制御によって目標慣性力モーメン ト、 目標重力モー メントおよび実全床反カモ一メントが修正され、 常に条件 1を満たしていること を意味する。

同一のことを式 1 3を用いて言い換えると、 対象物反力モーメ ン ト偏差が発生 しても、 対象物反力平衡制御によって、 目標慣性力モーメ ン ト摂動量、 目標重力 モ一メ ント摂動量および実全床反カモ一メント摂動量が発生し、 条件 1を満足す るように対象物反力モーメ ント偏差の影響を打ち消していると言える。

第 4図の台車押し作業の状況に対する対象物反力平衡制御の挙動を、 第 7図を 再び参照して説明する。

台車を押す作業を行っているとき、 台車 （即ち、 目標対象物） の実際のころが り摩擦力の絶対値が想定していた値よりも突然ステップ状に小さくなつてしまつ た状況では、 対象物反力モーメ ン ト偏差も、 図のようにステップ状に変化する。 これに対し、 最終到達目標重心位置摂動量算出部が、 最終到達目標重心位置摂 動量を算出する。 モデル制御則演算器により最終到達目標重心位置摂動量と目標 重心位置摂動量の差に応じて、 対象物反力平衡制御用補償全床反力モーメ ントが 算出 2·れる。

第 7図に示すように、 対象物反力平衡制御用補償全床反力モーメ ン トは、 目標 重心位置摂動量が最終到達目標重心位置摂動量に漸近するにつれて 0に漸近する 。 摂動動力学モデルには対象物反力平衡制御用補償全床反力モーメ ン トと対象物 反力モーメ ント偏差の和が入力され、 目標重心位置摂動量と目標上体位置 ·姿勢 摂動量が、 摂動動力学モデルから出力される。 尚、 この例では、 姿勢を変えない ことが拘束条件であるので、 目標上体位置 ·姿勢摂動量は 0である。

ところで、 摂動動力学モデルが動力学的平衡条件を満足することから、 目標上 体位置 ·姿勢摂動量によって発生する目標慣性力モーメント摂動量と目標重力モ ーメ ント摂動量の和に、 摂動動力学モデルに入力されたモーメントを加えた総和 は 0である。

即ち、 目標慣性力モーメン ト摂動量、 目標重力モーメン ト摂動量、 対象物反力 平衡制御用補償全床反力モーメントと対象物反力モーメント偏差の和は 0になる 。 この関係は、 第 7図に示すように、 常に成立する。 目標重心位置摂動量は、 モ デル制御則演算器によつて最終到達目標重心位置摂動量に漸近させられる。 目標 重心位置摂動量は、 目標重力モーメント摂動量に比例あるいはほぼ比例して変化 する。

以上が、 対象物反力平衡制御装置の挙動である。 尚、 上記制御演算は、 全て、 制御周期毎に実行される。 従って、 実対象物反力モーメン トがいつ変化しても、 常に、 動バランスは維持される。 換言すれば、 対象物反力モーメントが目標値か らずれたとき、 ロボッ ト 1は最初は全床反力モーメン トを操作してつまさき （足 平 2 2 R ( L ) の先端） を踏ん張るように姿勢制御されると共に、 経時的に上体 を前方に移動させて重力モーメントに頼るように切り換えられる。

第 1 2図はこの発明の第 2の実施の形態を示し、 リ ミ ッタ 2 0 0を設け、 最終 到達目標重心位置摂動量に上限下限の制限値を設定してリ ミ ッ トをかけるように したものである。

上記した実施の形態において、 実際には、 最終到達目標重心位置摂動量をあま り大きくすると、 ロボッ トの姿勢がとれなくなる場合が生じる。 従って、 これを 防ぐために、 式 7によって得られた最終到達目標重心位置摂動量に、 上限下限の 制限値 （範囲） を設定してリ ミ ッ トをかけるようにした。 尚、 その制限値 （リ ミ ッ ト値） は固定値でも良く、 あるいは可変値としても良い。

更に、 第 2のリ ミ ッタ 3 0 0を設け、 モデル制御則演算器で演算された対象物 反力平衡制御用補償全床反力モーメ ン トにも上限下限の制限値 （範囲） を設定し 、 リ ミ ツ トをかけるようにした。

即ち、 対象物反力平衡制御用補償全床反力モーメ ン トは実ロボッ トの足平 2 2 R ( L ) にも発生させるが、 実ロボッ トの足平が発生できる全床反力モーメント には限度があり、 限度を超えると、 足平の接地性が損なわれたり、 足平の一部が 床から浮いたりする。 それを防ぐためには、 モデル制御則演算器が式 1 0を用い て演算した対象物反力平衡制御用補償全床反力モーメ ン トに上限下限の制限値を 設定し、 リ ミ ッ トをかけるようにした。 その制限値が固定でも可変でも良いこと はリ ミ ッタ 2 0 0の場合と同様である。

第 1 3図はこの発明の第 3の実施の形態を示し、 リ ミ ッタ 4 0 0を設けて最終 到達目標重心位置摂動量算出部の入力に上限下限の制限値を設定してリ ミ ッ トを 力、けると共に、 第 2のリ ミ ッタ 5 0 0を設け、 第 1のリ ミ ッタ 4 0 0を超えた入 力値で対象物反力平衡制御用補償全床反力モーメ ン トを修正するようにした。 第 1 2図に示した第 2の実施の形態においては、 最終到達目標重心位置摂動量 が過大になるのを防止するために、 式 7によって得られた最終到達目標重心位置 摂動量に、 上限下限の制限値 （範囲） を設定してリ ミ ッ トをかけるようにしたが 、 制限値 （リ ミ ツ ト値） を超えた値がモデル制御則演算器の出力に加算点 6 0 0 (第 1 2図） において加算され、 モデルに入力されることがあった。 このため、 対象物反力モーメ ント偏差に抗して重心位置を摂動していたものが、 対象物反力 が過大になり、 リ ミ ッ夕が作動すると、 重心位置を逆方向に揺動してしまうと言 う不都合を生じることがあった。

第 3の実施の形態は上記した不都合を解消するもので、 第 1のリ ミ ッタ 4 0 0 の制限値 （リ ミ ツ ト値） を超えた入力値を第 2のリ ミツ夕 5 0 0を通して加減算 点 7 0 0に送り、 そこでモデル制御則演算器の出力から減算、 換言すれば極性を 反転させて転倒させようとする力と逆向きの力を与えるように、 対象物反力平衡 制御用補償全床反力モーメン トを修正するようにした。 これによつて、 ロボッ ト は制限値 （リ ミ ツ ト値） を超えた入力値 （モ一メ ント偏差） を足平 2 2 R ( L ) で支持するように姿勢制御される。

第 2の実施の形態と異なり、 リ ミ ッ ト値を超えた値がモデル制御則演算器の出 力に加算点 7 1 0 (第 1 3図） において加算されてモデルに入力されることがな レ、。 このため、 前記したような対象物反力モーメン ト偏差が過大になってリ ミ ツ 夕が作動し、 重心位置を逆方向に揺動してしまうと言う不都合を解消することが できる。 尚、 残余の構成は従前の実施の形態と異ならない。 第 1のリ ミ ッタ 4 0 0の制限値 （リ ミ ツ ト値） が固定でも可変でも良いことも、 従前の実施の形態と 同様である。 さらに、 モデル制御則演算器の後に、 リ ミ ッタ 4 0 0 と同様のリ ミ ッ夕を追加しても良い。

第 1 4図はこの発明の第 4の実施の形態を示し、 対象物反力平衡制御装置にお いて摂動動力学モデルの精度を高めるために、 慣性力モーメント Iを与えた倒立 振子モデルを使用するようにした。

更に、 摂動動力学モデルについて敷衍すると、 重心高さがあまり変わらないな らば、 hは固定で良いが、 作業によって重心高さが変わる場合には、 最終修正目 標上体位置 ·姿勢、 最終修正目標足平位置 ·姿勢および最終修正目標ハンド位置 •姿勢から求められるロボッ ト姿勢から重心高さを求め、 これに応じて hを変更 しても良い。

また、 摂動動力学モデルの精度をより高めるために、 脚腕のリンクを持つロボ ッ 卜の多リ ンク幾何学モデルを備え、 最終修正目標上体位置 ·姿勢、 最終修正目 標足平位置 ·姿勢および最終修正目標ハンド位置 ·姿勢から求められる重心位置 と、 最終修正目標上体位置 ·姿勢から上体位置の摂動量を差し引いた上体位置 · 姿勢、 最終修正目標足平位置 ·姿勢および最終修正目標ハンド位置 ·姿勢から求 められる重心位置との差を求めることにより、 高精度な重心位置の摂動量と上体 位置の摂動量との関係を求め、 それを用いて重心位置の摂動量から上体位置の摂 動量を求めても良い。

また、 脚腕のリ ンクを持つロボッ トの多リンク動力学モデルであって、 運動パ ターンにある拘束条件を与えておいて、 目標床反力の摂動を入力として目標上体 位置 ·姿勢摂動量と重心位置 ·姿勢摂動量を出力させるモデルを用いても良い。

ところで、 腕を目標作業パターンから摂動させたときの腕の慣性力摂動量およ びまたは重力摂動量の影響を考慮した摂動動力学モデルを用いると、 制御装置の 負荷が大きくなる。

何故なら、 腕の慣性力摂動量およびまたは重力摂動量は、 目標上体位置 ·姿勢 摂動量に影響され、 逆に目標上体位置 ·姿勢摂動量は、 腕の慣性力摂動量および または重力摂動量に影響されるので、 この相互作用を同時に考慮してモデル挙動 を算出することは、 非常に複雑な演算が必要となるからである。

その問題を解決する手段として、 以下の手法を用いても良い。

即ち、 摂動動力学モデルでは、 腕を目標作業パターンから摂動させたときの腕 の慣性力摂動量およびまたは重力摂動量の影響を無視し、 腕は目標動作パターン 通りにしか動かないものと仮定する。 この仮定により、 モデルは摂動動力学モデ ルの詳細説明に例として挙げた倒立振子モデルと同一形式に近似される。 従って 、 摂動動力学モデルの演算は極めて簡単になる。

腕メイン制御装置において、 目標ハン ド位置 ·姿勢、 目標上体位置、 最終修正 目標ハンド位置 ·姿勢および最終修正目標上体位置から、 腕を目標姿勢から最終 目標姿勢に摂動させたために生じた慣性力摂動量およびまたは重力摂動量を算出 する。 これは、 従来からの手法である多リンクマニピュレータの動力学演算を行 うことにより得られる。 これは、 最終修正目標ハンド位置 ·姿勢座標系で表す。 算出された惯性力摂動量およびまたは重力摂動量を、 センサによって検出され た実対象物反力に加え、 対象物反力平衡制御装置に実対象物反力として出力する 。 以上の手段により、 腕の慣性力摂動量およびまたは重力摂動量の影響を、 摂動 動力学モデルにおいて無視した代わりに、 作業対象物反力として考慮したことと なる。 腕の慣性力摂動量およびまたは重力摂動量の演算と摂動動力学モデルの演 算が独立して行われるので、 複雑な干渉演算が不要となり、 演算量が小さくて済 む。

上記の如く、 第 1ないし第 4の実施の形態にあっては、 少なく とも基体 （上体 2 8 ) と、 前記基体に連結される複数本のリンク （脚リンク 2および腕リ ンク 3 ) からなる脚式移動ロボッ ト （ 2足歩行ロボッ ト 1 ) の姿勢制御装置において、 前記ロボッ トの少なく とも前記基体の目標軌跡を含む運動パターンと、 前記ロボ ッ トに作用する床反力の目標軌跡と、 前記ロボッ トに作用する床反力以外の外力 の目標軌跡とを少なく とも含む、 前記ロボッ 卜の目標歩容を設定する目標歩容設 定手段 （目標作業パターン生成器） 、 前記床反力以外の外力を検出する外力検出 手段 （ 6軸力センサ 5 8 ) 、 前記検出された外力と、 前記目標軌跡で設定された 床反力以外の外力の偏差 （目標全床反力中心点まわりの対象物反力モーメント偏 差） を演算する外力偏差演算手段 （対象物反力平衡制御装置。 より具体的には、 第 8図の実対象物反力の座標変換およびその入出力） 、 前記床反力の摂動と前記 ロボッ トの重心位置および Zまたは基体の位置の摂動の関係を表現するモデル （ 摂動動力学モデル） 、 少なく とも前記演算された外力の偏差に基づいて前記乇デ ルに入力すべきモデル入力量 （モデルのための目標全床反力モーメント摂動量） を演算するモデル入力量演算手段 （モデル制御則演算器およびその後の加算点で の入出力） 、 前記演算されたモデル入力量を前記モデルに入力し、 得られる前記 重心位置および/または基体の摂動量に応じて前記基体の目標軌跡を修正する、 基体目標軌跡修正量 （修正目標上体位置，姿勢） を演算する基体目標軌跡修正量 演算手段 （対象物反力平衡制御装置。 より具体的には、 摂動動力学モデル入力量 を入力し、 モデルの挙動を演算し、 モデル出力から目標上体位置姿勢摂動量 （修 正量） を求める部分） 、 少なく とも前記演算されたモデル入力量に応じて前記床 反力の目標軌跡を修正する、 床反力目標軌跡修正量 （対象物反力平衡制御用補償 全床反力モーメント） を演算する床反力目標軌跡修正量演算手段 （モデル制御則 演算器。 より具体的にはモデル制御則の一部） 、 および少なく とも前記演算され た基体目標軌跡修正量および床反力目標軌跡修正量に基づいて前記ロボッ トの関 節を変位させる関節変位手段 （脚メイン制御装置、 ァクチユエ一夕駆動装置 8 8

、 脚ァクチユエ一夕など） 、 を備える如く構成した。

また、 前記モデル入力量演算手段は、 前記外力に静的に平衡する平衡重心位置 の摂動量を算出する平衡重心位置摂動量算出手段 （最終到達目標重心位置摂動量 算出部） 、 を備え、 前記算出された平衡重心位置に前記モデルが収束するように 前記モデル入力量を演算する如く構成した。

また、 前記モデルが前記ロボッ トを倒立振子で近似するモデル （摂動動力学モ デル） である如く構成した。

また、 前記平衡重心位置摂動量算出手段は、 前記算出された平衡重心位置の摂 動量を所定の範囲に制限するリ ミ ツ夕 2 0 0， 4 0 0を備える如く構成した。

また、 前記床反力目標軌跡修正量演算手段は、 前記演算された床反力目標軌跡 修正量を所定の範囲に制限するリ ミ ッタ 3 0 0， 5 0 0を備えるように構成した o

また、 前記床反力の目標軌跡は、 前記ロボッ 卜に作用する床反力の目標中心点 の軌跡を少なく とも含む如く構成した。

また、 前記床反力目標軌跡修正量演算手段は、 前記床反力目標軌跡修正量 （対 象物反力平衡制御補償用全床反力モーメント） 力 \ 前記モデル入力量 （モデルの ための目標全床反力モーメント摂動量） から前記外力の偏差 （目標全床反力中心 点まわりの対象物反力モーメント偏差） を減算した値と、 前記床反力の目標中心 点まわりに作用するモーメン卜に動力学的に釣り合うように、 前記床反力目標軌 跡修正量を演算する如く構成した。

また、 前記床反力以外の外力が、 前記リンクを介して前記ロボッ 卜に作用する 作業対象物 （台車 1 0 0 ) からの反力である如く構成した。

また、 前記ロボッ トが、 前記基体に連結される 2本の脚リンク 2と 2本の腕リ ンク 3からなる脚式移動ロボッ トである如く構成した。

また、 少なく とも基体 （上体 2 8 ) と、 前記基体に連結される複数本のリ ンク (脚リ ンク 2、 腕リンク 3 ) からなる脚式移動ロボッ トの姿勢制御装置において 、 前記ロボッ トの少なく とも前記基体の目標位置を含む運動パターンと、 前記口 ボッ トに作用する床反力の目標中心点の軌跡を少なく とも含む、 前記ロボッ トの 目標歩容を設定する目標歩容設定手段 （目標作業パターン生成器） 、 前記リ ンク を介して前記ロボッ トに作用する、 作業対象物からの反力を検出する対象物反力 検出手段 （ 6軸力センサ 5 8 ) 、 前記検出された対象物反力を前記目標床反力中 心点まわりのモ一メントとして変換する対象物反力モーメント変換手段 （対象物 反力平衡制御装置） 、 前記変換された対象物反力モーメン トに動力学的に釣り合 うように、 前記目標中心点まわりの床反力モ一メントと前記ロボッ トの位置およ び姿勢を修正するロボッ ト位置，姿勢修正手段 （対象物反力平衡制御装置） 、 お よび前記修正された目標中心点まわりの床反力モーメントと前記ロボッ トの位置 •姿勢に基づいて前記ロボッ トの関節を変位させる関節変位手段 （脚メイン制御 装置、 ァクチユエ一夕駆動装置 8 8、 脚ァクチユエ一夕など） 、 を備える如く構 成した。

また、 少なく とも基体 （上体 2 8 ) と、 前記基体に連結される複数本のリ ンク (脚リ ンク 2、 腕リ ンク 3 ) からなる脚式移動ロボッ トの姿勢制御装置において 、 前記ロボッ トの少なく とも前記基体の目標位置を含む運動パターンを設定する 目標歩容設定手段 （目標作業パターン生成器） 、 前記リンクを介して前記ロボッ トに作用する、 作業対象物からの反力を検出する対象物反力検出手段 （ 6軸カセ ンサ 5 8 ) 、 前記検出された対象物反力を所定の点、 より具体的には目標床反力 中心点まわりのモーメントとして変換する対象物反力モーメント変換手段 （対象 物反力平衡制御装置） 、 前記変換された対象物反力モーメン トに動力学的につり あうように、 前記所定の点まわりの床反力モ一メントと前記ロボッ トの位置およ び姿勢を修正するロボッ ト位置 ·姿勢修正手段 （対象物反力平衡制御装置） 、 お よび前記修正された所定の点まわりの床反力モーメントと前記ロボッ 卜の位置 - 姿勢に基づいて前記ロボッ トの関節を変位させる関節変位手段 （脚メイン制御装 置、 ァクチユエ一夕駆動装置 8 8、 脚ァクチユエ一夕など） 、 を備える如く構成 した。

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

上記した第 1ないし第 4の実施の形態では、 先に特開平 5 - 3 0 5 5 8 6号公 報で提案したコンプライアンス制御を用いているが、 それ以外の手段を用いても 良い。 脚制御に、 そのコンプライアンス制御以外の別の手段、 たとえば、 電動ァ クチユエ一夕を電流指令型のァンプによつて制御する手段を用いて関節トルクを 制御し、 その結果、 間接的に床反力を制御する手段を用いれば、 足平 2 2 R ( L ) に設けた 6軸力センサ 5 6は不要である。

更に、 上記した第 1ないし第 4の実施の形態において、 腕の制御に、 仮想コン プライアンス制御以外の別の手段、 たとえば、 電動ァクチユエ一夕を電流指合型 のアンプによって制御する手段を用いて関節トルクを制御し、 その結果、 間接的 に対象物反力を制御しても良い。 その制御ではハンドの 6軸力センサは不要であ るが、 対象物反力平衡制御装置のために、 ハンドの 6軸力センサは設けるのが良 い。

更に、 上記した第 1ないし第 4の実施の形態において、 ハンドの 6軸力センサ の代わりに、 関節トルクから実対象物反力を推定する推定器を腕制御装置に備え ても良い。 この推定器は、 従来技術である外乱オブザーバ一を用いれば良い。 更には、 上記した第 1ないし第 4の実施の形態において、 特開平 5 - 3 0 5 5 8 6号で提案したコンプライアンス制御に加えて、 本出願人が特開平 5 - 3 3 7 8 4 9号公報で提案した制御を加えても良い。 但し、 その制御によって上体の位 置や歩幅が修正されるので、 腕制御においてハンドと作業対象物との相対位置関 係が重要な場合には、 その制御によって修正される上体の位置や歩幅の影響を考 慮する必要がある。

更には、 上記した第 1ないし第 4の実施の形態において、 床が平面でない場合 でも、 本出願人が特開平 5 - 3 1 8 8 4 0号公報で提案した仮想平面を想定する 技術を用いて目標全床反力中心点や目標 Z M Pを仮想平面上に求めても良い。 更には、 上記した第 1ないし第 4の実施の形態において、 ロボッ ト全体の姿勢 が目標からずれて傾く と、 ハンドの位置■姿勢が絶対空間においてずれる。 この 結果、 対象物反力が目標対象物反力から大きくずれる場合がある。

その問题点を解決するために、 傾斜センサによって検出される実上体位置 ·姿 勢と目標上体位置 ·姿勢のずれに応じて上記の修正された最終目標ハンド位置 · 姿勢をさらに補正することにより、 ロボッ ト全体の姿勢が傾いても、 ハンドの位 置 ·姿勢が絶対空間においてずれないようにするのが、 より好ましい。

更には、 上記した第 1ないし第 4の実施の形態において、 ブロック図は演算処 理順序を変えるなど、 種々の変形が可能である。

更には、 上記した第 1ないし第 4の実施の形態においては P D制御則を用いた 力、 それ以外の制御則 （たとえば、 P ID制御、 状態フィードバック制御） などを 用いても良い。

また、 この発明を腕を備えた 2足歩行の脚式移動ロボッ トについて説明したが 、 腕を備えない脚式移動ロボッ トにも有益であり、 更に 2足歩行ロボッ トに限ら ず、 多脚ロボッ トにも応用することができる。 産業上の利用可能性

この発明によれば、 脚式移動ロボッ トが予期できない外力、 より具体的には作 業対象物から反力を受けても、 動バランスをとつて安定な姿勢を継続することが できる。 更に、 予め想定していなかった動作パターンでリンク、 より具体的には 腕を動かして作業をするときに腕に発生する重力、 慣性力だけでなく、 作業対象 から予期せぬ反作用を受けても、 動バランスをとつて安定な姿勢を継続すること ができる。

また、 対象物反力が急変するときも、 それに静的にバランスする位置にロボッ トの重心を移動させることにより、 傾きや転倒を効果的に抑制することができる 。 また、 ロボッ トの重心が移動する過渡期においても、 重心位置や床反力を適正 に変化させて動バランスを維持し続けることができる。

Claims

請求の範囲

1 . 少なくとも基体と、 前記基体に連結される複数本のリンクからなる脚式移動 ロボッ トの姿勢制御装置において、

a . 前記ロボッ トの少なくとも前記基体の目標軌跡を含む運動パターンと、 前記 ロボッ トに作用する床反力の目標軌跡と、 前記ロボッ トに作用する床反カ以 外の外力の目標軌跡とを少なくとも含む、 前記ロボッ トの目標歩容を設定す る目標歩容設定手段、

b . 前記床反力以外の外力を検出する外力検出手段、

c 前記検出された外力と、 前記目標軌跡で設定された床反力以外の外力の偏差 を演算する外力偏差演算手段、

d . 前記床反力の摂動と前記ロボッ トの重心位置および Zまたは基体の位置の摂 動の関係を表現するモデル、

e . 少なくとも前記演算された外力の偏差に基づいて前記モデルに入力すべきモ デル入力量を演算するモデル入力量演算手段、

f . 前記演算されたモデル入力量を前記モデルに入力し、 得られる前記重心位置 および/または基体の摂動量に応じて前記基体の目標軌跡を修正する、 基体 目標軌跡修正量を演算する基体目標軌跡修正量演算手段、

g . 少なくとも前記演算されたモデル入力量に応じて前記床反力の目標軌跡を修 正する、 床反力目標軌跡修正量を演算する床反力目標軌跡修正量演算手段、 および

h . 少なくとも前記演算された基体目標軌跡修正量および床反力目標軌跡修正量 に基づいて前記ロボッ トの関節を変位させる関節変位手段、

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

2 . 前記モデル入力量演算手段は、

j . 前記外力に静的に平衡する平衡重心位置の摂動量を算出する平衡重心位置摂 動量算出手段、

を備え、 前記算出された平衡重心位置に前記モデルが収束するように前記モデル 入力量を演算することを特徴とする請求項 1項記載の脚式移動口ボッ トの姿勢制 御装置。

3 . 前記モデルが前記ロボッ トを倒立振子で近似するモデルであることを特徴と する請求項 1項または 2項記載の脚式移動ロボッ トの姿勢制御装置。

4 . 前記平衡重心位置摂動量算出手段は、

k . 前記算出された平衡重心位置の摂動量を所定の範囲に制限するリ ミ ッ夕、 を備えることを特徴とする請求項 2項または 3項記載の脚式移動ロボッ トの姿勢 制御装置。

5 . 前記床反力目標軌跡修正量演算手段は、

1 . 前記演算された床反力目標軌跡修正量を所定の範囲に制限するリ ミッ夕、 を備えることを特徴とする請求項 1項ないし 4項のいずれかに記載の脚式移動口 ボッ トの姿勢制御装置。

6 . 前記床反力の目標軌跡は、 前記ロボッ トに作用する床反力の目標中心点の軌 跡を少なくとも含むことを特徴とする請求項 1項ないし 5項記載の脚式移動ロボ ッ トの姿勢制御装置。

7 . 前記床反力目標軌跡修正量演算手段は、 前記床反力目標軌跡修正量が、 前記 モデル入力量から前記外力の偏差を減算した値と、 前記床反力の目標中心点まわ りに作用するモーメントに動力学的に釣り合うように、 前記床反力目標軌跡修正 量を演算することを特徴とする請求項 6項記載の脚式移動口ボッ トの姿勢制御装

8 . 前記床反力以外の外力が、 前記リンクを介して前記ロボッ トに作用する作業 対象物からの反力であることを特徴とする請求項 1項ないし 7項のいずれかに記 載の脚式移動ロボッ 卜の姿勢制御装置。

9 . 前記ロボッ ト力、 前記基体に連結される 2本の脚リ ンクと 2本の腕リ ンクか らなる脚式移動ロボッ トであることを特徴とする請求項 1項ないし 7項のいずれ かに記載の脚式移動ロボッ トの姿勢制御装置。

1 0 . 少なく とも基体と、 前記基体に連結される複数本のリンクからなる脚式移 動ロボッ トの姿勢制御装置において、

a . 前記口ポッ トの少なく とも前記基体の目標位置を含む運動パターンと、 前記 ロボッ トに作用する床反力の目標中心点の軌跡を少なく とも含む、 前記ロボ ッ トの目標歩容を設定する目標歩容設定手段、

b . 前記リ ンクを介して前記ロボッ トに作用する、 作業対象物からの反力を検出 する対象物反力検出手段、

c 前記検出された対象物反力を前記目標床反力中心点まわりのモーメ ントとし て変換する対象物反力モーメ ン ト変換手段、

d . 前記変換された対象物反力モーメ ン トに動力学的に釣り合うように、 前記目 標中心点まわりの床反力モーメントと前記ロボッ 卜の位置および姿勢を修正 するロボッ ト位置 ·姿勢修正手段、

および

e . 前記修正された目標中心点まわりの床反力モーメントと前記ロボッ トの位置

-姿勢に基づいて前記ロボッ 卜の関節を変位させる関節変位手段、 を備えたことを特徴とする脚式移動 oボッ トの姿勢制御装置。

1 1 . 少なく とも基体と、 前記基体に連結される複数本のリンクからなる脚式移 動ロボッ トの姿勢制御装置において、

a . 前記ロボッ トの少なく とも前記基体の目標位置を含む運動パターンを設定す る目標歩容設定手段、

b . 前記リンクを介して前記ロボッ トに作用する、 作業対象物からの反力を検出 する対象物反力検出手段、

c 前記検出された対象物反力を所定の点まわりのモーメントとして変換する対 象物反力モ--メント変換手段、 d . 前記変換された対象物反力モーメン トに動力学的につりあうように、 前記所 定の点まわりの床反力モ一メントと前記ロボッ トの位置および姿勢を修正す るロボッ ト位置 ·姿勢修正手段、

および

e . 前記修正された所定の点まわりの床反力モ一メントと前記ロボッ トの位置 . 姿勢に基づいて前記ロボッ トの関節を変位させる関節変位手段、

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