WO2008001713A1 - Robot articulé et son programme de commande - Google Patents

Description

明 細 書

多関節ロボット及びその制御プログラム

技術分野

[0001] 本出願は、 2006年 6月 26日に出願された日本国特許出願第 2006— 175063号 に基づく優先権を主張する。その出願の全ての内容はこの明細書中に参照により援 用されている。

本発明は、多関節ロボットとその制御プログラム（多関節ロボットを制御するためのコ ンピュータプログラムプロダクト）に関する。

背景技術

[0002] 多関節ロボットは、多関節ロボット機構と、その機構を制御するためのコントローラを 備える。本明細書では、「多関節ロボット機構」を表す場合に「多関節ロボット」或いは 単に「ロボット」と称することがある。本明細書にいう多関節ロボット機構は、複数のリン クと複数の関節を有しており、隣接するリンクが関節を介して連結されており、基部リ ンクと先端リンクの間に少なくとも 2以上の関節が介在しているロボットをいう。ここでい う先端リンクとは目標状態ベクトルが与えられるリンクを意味し、必ずしも物理的に先 端に配置されるリンクに限らない。例えば、 5指を含む腕を模擬した多関節ロボットで あっても、掌リンクの目標状態ベクトルが与えられる場合は、掌リンクの先に指リンク群 が連結されていても掌リンクが先端リンクとなる。また本明細書では 1自由度を 1関節 とカウントし、隣接するリンクを 2軸周りに回転可能な関節は 2つの関節から構成され ているとする。

[0003] n個の関節を有する多関節ロボット（即ち、 n自由度を有する多関節ロボット）は、各 関節を独立に制御することによって、先端リンクの現在状態ベクトルを、 n以下の次元 で記述される目標状態ベクトルに追従にさせることができる。本明細書では「先端リン クの現在状態ベクトルを目標状態ベクトルに追従にさせる」との表現を単純化して「先 端リンクを目標状態ベクトルに追従させる」と表現する。目標状態ベクトルは、位置（ 姿勢)の単位で与えられることもあれば、速度 (角速度）の単位で与えられることもある 。或いは加速度 (加速度）の単位や力（トルク）の単位で与えられることもある。例えば 、目標状態ベクトルが 6次元を有しており、位置 (姿勢)の単位で与えられる場合には 、目標状態ベクトルの要素は直交座標系の各座標軸方向の位置と、所謂ロール角、 ピッチ角、及びョ一角の 6個のスカラー値で与えられる。 目標状態ベクトルの要素は 様々な単位系を取り得るため、本明細書では目標位置ベクトルや目標速度ベクトル などと 、う表現の代わりに単位系を特定しな ヽ目標状態ベクトルと 、う表現を用いる。 先端リンクの現在状態ベクトルは、先端リンクの物理的状態を、目標状態ベクトルと同 じ単位系で記述したベクトルである。

なお、目標状態ベクトルの次数を n以下と表現したのは、本明細書で扱う多関節口 ボットは、いわゆる冗長自由度を有する多関節ロボットであってもよいからである。

[0004] n自由度の多関節ロボットの先端リンクに対して n以下の目標状態ベクトルが与えら れると、先端リンクを目標状態ベクトルに追従させるための各関節の目標駆動量を決 定することができる。関節の目標駆動量の単位は、位置 (角度)や速度 (角速度)など 、様々な単位系を取り得る。本明細書における「目標駆動量」との表現は、関節の目 標関節角や目標角速度といった、関節の「目標状態」を含む意味で用いる。従って、 例えば関節の現在関節角を目標関節角に追従するように関節を駆動することを、「関 節を目標駆動量に追従させる」と表現する場合がある。

先端リンクを目標状態ベクトルに追従させるための各関節の目標駆動量は、いわゆ る逆変換やヤコビ行列によって求められることは良く知られている。関節の目標駆動 量が求まると、関節の現在状態 (現在関節角や現在関節角速度など)が関節の目標 駆動量（目標関節角や目標関節角速度など）に追従するように、関節を駆動するた めのァクチユエータが制御される。ここに「関節を駆動する」とは、関節に連結されたリ ンク同士の相対位置 (姿勢)を変化させることである。回転関節の場合は関節に連結 されたリンク間の角度 (関節角）を変化させることに相当する。直動関節の場合は関 節に連結されたリンクの間の距離を変化させることに相当する。

[0005] ロボットの先端リンクを目標状態ベクトルに追従させることによって、ロボットに所定 の作業を行わせることができる。例えばグリッパや溶接ツールなどの手先効果器を口 ボットの先端リンクに固定し、先端リンクを経時的に変化する目標状態ベクトルに追従 させることによって、その手先効果器を所望の軌道に沿って移動させて所定の作業 を行わせることができる。或いは、多関節ロボットが脚式ロボットである場合には、先 端リンクに相当する足平リンクを目標状態ベクトルに追従させることによって、歩行と

V、う作業を実行させることができる。

[0006] ところで、ロボットの作業中に、関節に作用する負荷が増大する可能性がある。例え ば、ロボットが予期せぬ物体に接触したにもかかわらず、時間的に変化する目標状 態ベクトルに追従する制御を継続すると、結果的にロボットが物体を押しのけることに なる。ロボットが物体を押しのける際に関節に作用する負荷が増大する恐れがある。 あるいは、ロボットが先端リンクに取り付けられたグリッパによって物体を持ち上げる作 業を行う場合に、持ち上げる物体の質量が予想以上に大きいと、関節に作用する負 荷も予想以上に増加する。関節に作用する負荷のうち、関節の駆動軸に作用してい る負荷成分については、その関節を駆動するァクチユエータにトルクリミッタなどの過 負荷対策を設けることによって緩和することができる。以下では関節の駆動軸に作用 する負荷を対象とする。また、本明細書にいう過負荷とは、関節に作用する負荷が予 め設定された閾値を超える場合を意味する。即ち、本明細書にいう過負荷とは、関節 の物理的な許容限度を超える負荷が関節に作用する場合に限られない。

過負荷対策として関節に装備されるトルクリミッタにはメカ-カルなトルクリミッタとソ フトウェア的なトルクリミッタがある。特許文献 1には、ソフトウェア的なトルクリミッタの 一例が開示されている。特許文献 1に開示された技術は、ァクチユエータであるモー タに印加される電流を監視し、印加された電流が所定値を超えた場合にモータへの 電流の供給を停止すると!、う技術である。

特許文献 1 :日本国特許公開公報 2005— 161469

発明の開示

発明が解決しょうとする課題

[0007] ある関節が過負荷となりトルクリミッタが作動すると、その関節は目標駆動量に基づ

V、て制御することができなくなる。

前述したように、多関節ロボットでは、先端リンクを目標状態ベクトルに追従させるよ うに各関節の目標駆動量が決定される。目標状態ベクトルの夫々の要素の値は、通 常、複数の関節の目標駆動量の関数となる。しかも、ロボットの全ての関節が先端リ ンクを目標状態ベクトルに追従させるために利用できるという前提で個々の関節の目 標駆動量が決定される。従って、ひとつでも関節が過負荷となり、その関節を目標駆 動量に追従させることができなくなると、 目標状態ベクトルの複数の要素について、先 端リンクの現在状態ベクトルの要素の値を対応する目標状態ベクトルの要素の値に 追従させることができなくなる。即ち、ロボットの作業が中断してしまう。

[0008] ひとつの関節が過負荷になっても他の関節が過負荷となっているとは限らない。む しろ過負荷になって 、な 、関節の数が、過負荷になった関節よりも多 、可能性が高 い。過負荷になっていない関節が残っているならば、それらの関節を適切に駆動す ることによって、ロボットの先端リンクを目標状態ベクトルに追従させる制御をできるだ け継続し、ロボットに予定して 、た作業を続行させることが好ま 、。

従来の技術では、ひとつの関節でも過負荷となれば、過負荷状態を解消することの みに着目しており、過負荷状態を解消しながら、予定していた作業をできるだけ継続 させるものでない。関節が過負荷となった場合に、その過負荷状態を解消しながら、 過負荷となっていない関節を活用し、予定されたロボット作業を継続し得る多関節口 ボットが望まれている。

課題を解決するための手段

[0009] ロボットの先端リンクの現在状態ベクトルを与えられた目標状態ベクトルに追従させ るには、 目標状態ベクトルの要素の数に対して、任意の関節角に制御可能な関節の 数がそれ以上であることが要求される。即ち、 n次元の目標状態ベクトルに対しては口 ボットには n以上の自由度が要求される。そこで通常は、 目標状態ベクトルの次元数 に応じてロボットの自由度数が決定される。例えば目標状態ベクトル力 次元であれ ば n自由度を有する多関節ロボットが用意される。ロボットの作業中にある関節が過 負荷となった場合、その関節はもはや先端リンクを目標状態ベクトルに追従させるた めの制御には利用できないので、先端リンクを目標状態ベクトルに追従させる制御に 利用できる関節数が n個を下回ることになる。その結果、ロボットの先端リンクを目標 状態ベクトルへ追従させることができなくなる。

[0010] 発明者は、ロボットに行なわせる作業の種類に応じて、先端リンクの目標状態べタト ルの要素には、先端リンクをできるだけ正確に追従させたい要素と、それほど正確に 追従させなくともよい要素があることに着目した。例えば、先端リンクに取り付けられた 手先効果器で水の入ったコップを把持して移動させる作業を考察する。このとき、目 標状態ベクトルは先端リンクの空間 6次元（3軸の直交座標系における各軸方向の目 標位置と各軸周りの目標姿勢角）の 6要素を有すると仮定する。先端リンクの目標姿 勢角はコップ内の水がこぼれないように設定されている。この場合、先端リンクの水平 面内の 2軸周りの姿勢角（即ち、コップの傾き角）のいずれか一方でも目標姿勢角か ら外れるとコップ内の水がこぼれてしまう可能性がある。一方、先端リンクの鉛直方向 の軸周りの姿勢角（即ち、コップの鉛直軸周りの回転角）は、目標姿勢角から外れて も水がこぼれることは無い。さらに鉛直軸周りの姿勢角が変化しても、コップの 3軸方 向の位置は変化しない。水をこぼさずにコップを移動させる作業において、先端リン クの鉛直軸周りの姿勢角が目標姿勢角に追従しなくとも、現在状態ベクトルの他の要 素 (即ち、先端リンクの 3軸方向の位置と水平面内の 2軸周りの姿勢角の 5要素）が目 標状態ベクトルの対応する要素に追従すれば、コップの移動作業は完遂することが できる。即ち、ある関節のァクチユエ一タが過負荷となった場合には (即ち、その関節 を、先端リンクを目標状態ベクトルに追従させる制御に利用できなくなった場合には） 、先端リンクの鉛直軸周りの現在姿勢角の目標姿勢角への追従性を犠牲にしても、 現在状態ベクトルの他の要素について、目標状態ベクトルの対応する要素への追従 性が維持されるように残りの関節を制御すれば作業を継続することが可能となる。 そこで本発明では、ロボットの先端リンクの目標状態ベクトルの各要素に対して優先 順位を設定しておく。 目標状態ベクトルの要素のうち、できるだけ正確に追従させた V、要素には高 、優先順位を設定し、高 、追従性が要求されな 、要素には低 、優先 順位を設定する。優先順位はロボットに行なわせる作業の種類に応じて事前に設定 すればよい。そうしておいて、ロボットの作業中に次の処理を行う。過負荷状態の関 節を特定し、その関節については目標状態ベクトルに関わらずに過負荷状態を解消 するように関節の目標駆動量を決定する。例えば、関節の目標駆動量が目標関節角 として設定されるならば、作用して 、る負荷の方向と同じ方向に関節を回転させるよう に目標関節角を決定すればよい。

一方、過負荷となっていない非過負荷関節の数を特定し、目標状態ベクトルから、 優先順位の高い順に非過負荷関節と同数だけ要素を抽出する。抽出した要素の数 と非過負荷関節の数は同数であるため、非過負荷関節については、抽出した要素を 実現するための目標駆動量を決定することができる。

[0011] 本発明は、多関節ロボットの自由度数は目標状態ベクトルの次数以上でなければ ならないとのこれまでの通念に対して、先端リンクを目標状態ベクトルに追従させる制 御に利用できる関節の数と同数まで目標状態ベクトルを低次元化するという新たな発 想に基づく。さらに、目標状態ベクトルの要素には追従性に関して優先順位があるこ とに着目し、目標状態べ外ルの各要素に優先順位を予め設定する。設定された優 先順位の低い順に切り捨てて目標状態ベクトルを低次元化することによって、作業に とって重要な要素（目標状態ベクトルの要素)の追従性を維持する。一方、過負荷関 節については、目標状態ベクトルに関わらず、過負荷状態を緩和するように関節の 目標駆動量を設定する。

過負荷関節の過負荷状態を緩和しながら当初に予定されていた作業をできるだけ «続し得る多関節ロボットを実現することに成功した。

なお、上記の議論は、目標状態ベクトルの各要素の単位系に依存しない。即ち、目 標状態ベクトルの要素は位置や角度（姿勢角）だけでなく速度や加速度であってもよ い。このことは、関節の目標駆動量が角度の単位に限られず角速度や角加速度ゃト ルクの単位であっても成立することを意味する。或いは、関節が直動関節の場合に は、関節の目標駆動量が伸展長さ、伸展速さ、伸展加速度、伸展させる力であっても 成立する。

[0012] 本発明は、多関節ロボット機構とコントローラを有する多関節ロボットに具現ィ匕でき る。多関節ロボット機構は、複数のリンクと複数の関節と複数のァクチユエータを有し ている。隣接するリンクが関節を介して連結されており、夫々のァクチユエータが夫々 の関節を駆動することによって、先端リンクの状態を変化させることが可能に構成され ている。

コントローラは、記憶部、負荷推定部、過負荷関節特定部、第 1駆動量算出部、第 2駆動量算出部、及びドライバを備える。

記憶部には先端リンクの目標状態ベクトルと、目標状態ベクトルの要素毎に設定さ れている優先順位が記憶されている。負荷推定部は、各関節の駆動軸に作用してい る負荷の大きさと方向を推定する。過負荷関節特定部は、推定した負荷の大きさが 閾値より大きい過負荷関節と、推定した負荷の大きさが閾値以下の非過負荷関節を 特定する。第 1駆動量算出部は、過負荷関節に対して、負荷の作用する方向と同じ 方向に過負荷関節を駆動する目標駆動量を算出する。一方、第 2駆動量算出部は、 目標状態ベクトルの要素の中から優先順位の高い順に非過負荷関節と同数の要素 を抽出し、非過負荷関節に対して、抽出した要素の値を実現するように非過負荷関 節を駆動する目標駆動量を算出する。そしてドライバは、第 1駆動量算出部と第 2駆 動量算出部が算出した目標駆動量に基づいてァクチユエータを制御する。

各関節の駆動軸に加わる負荷とは、回転関節の場合は駆動軸に加わるトルクであ り、直動関節の場合は駆動軸の伸びる方向に加わる力である。

負荷推定部は、回転関節の場合には関節軸に取り付けられたトルクセンサ力もの 信号を受信し、受信した信号に所定の係数を乗じて負荷の大きさと方向を推定する 演算部でよい。或いは、ァクチユエータが電流駆動のモータの場合には、ドライバが モータへ出力する電流指令値力 ァクチユエータの出力トルクの大きさと方向を推定 し、その出力トルクから駆動軸に加わる負荷の大きさと方向を推定する推定器でもよ い。電流駆動のモータは、印加される電流と出力トルクがほぼ比例関係を有する場 合が多い。そのようなモータの場合は、ドライバがモータへ出力する電流指令値にい わゆるトルク定数を乗じることで、モータの出力トルクを推定できる。モータの出力トル クに、モータ軸から関節駆動軸へのトルク伝達定数を乗じることによって、関節駆動 軸の出力トルクを推定できる。

また、関節駆動軸の出力トルクの大きさを負荷の大きさの推定値としてよい。出力ト ルクの方向に対して逆の方向を負荷の方向の推定値としてよい。或いは、先端リンク に力センサを取り付け、力センサが検出する外力の大きさと方向から各関節の駆動 軸に作用する負荷の大きさと方向を推定してもよい。力センサが検出する外力から各 関節の駆動軸に作用する負荷への変換は、良く知られているようにヤコビ行列を用 いて算出できる。

関節の目標駆動量は前述したように、回転関節の場合は関節角、関節角速度、関 節角加速度、あるいはトルクのいずれかであってよい。直動関節の場合は、伸展長さ 、伸展速さ、伸展加速度、或いは伸展させる力のいずれかであってよい。また、目標 状態ベクトルは経時的に変化するものであってもよい。また、閾値は関節ごとに異な る値でょ 、し、経時的に変化するものであってもよ!/、。

また、推定した負荷の大きさが閾値よりも大きい関節を過負荷関節と特定し、推定し た負荷の大きさが閾値以下である関節を非過負荷関節と特定する。ここで、「閾値よ りも大き 、」と「閾値以下である」との分類を、「閾値以上である」と「閾値未満である」と の分類に変更しても本発明の技術的範囲を超えるものではないことを明記する。本 発明では、閾値を設けることは重要であるが、閾値と同じ負荷が加わっている関節を 非過負荷関節に分類するのは、単に数学的に矛盾なく閾値の上下で関節を分類す るための便宜にすぎない。閾値と同じ負荷が加わっている関節を過負荷関節に分類 しても本発明の特徴はなんら変更されるものではない。

上記構成では、過負荷関節特定部が、全ての関節を、閾値を越える負荷が加わつ ている過負荷関節とそうでない非過負荷関節に分ける。過負荷関節については、第 1駆動量算出部によって、目標状態ベクトルに関わらずに負荷と同じ方向に過負荷 関節を駆動する目標駆動量が決定される。負荷が作用する方向と同じ方向に過負荷 関節を駆動すれば、負荷を低減できる可能性が高いからである。一方、過負荷となつ ていない非過負荷関節 (即ち、目標状態べ外ルへの追従制御に利用できる関節） については、第 2駆動量算出部によって、可能な範囲で先端リンクを目標状態べタト ルに追従させるように非過負荷関節を駆動する目標駆動量を決定する。具体的には 、非過負荷関節の数と同数の要素を目標状態ベクトルから抽出する。非過負荷関節 の数と抽出した要素数が一致するので、抽出した要素の値を実現することのできる目 標駆動量が決定できる。さらに、目標状態ベクトルの要素は、予め設定された優先順 位の高い順に抽出される。即ち、どの関節が過負荷となっても、追従性が重要視され る要素が常に抽出され、その要素については目標値への追従性が確保される。即ち 、予定されて!、た作業の動作に近!、動作が実現できる。

各関節の目標駆動量の決定は、目標駆動量が関節角で表される場合には逆変換 (逆運動学）により決定することができる。或いは順変換の繰り返し計算によっても決 定することができる。また目標駆動量が関節角速度で表される場合にはいわゆる分 解速度制御法に基づ 、て決定することができる。

ロボットに作業を行わせる際には、ロボットの先端リンクに手先効果器が取り付けら れている場合が多い。手先効果器はグリッパ（多指ハンドを含む）、切削ツール、溶接 ツールなど、作業目的に応じて選択される。そのような場合、手先効果器は対象物（ 例えば前述したコップ移動作業におけるコップなど)と接触するために、手先効果器 を介してロボットに外力が加わる。そのような外力は予定されている外力である。その 一方で、多関節ロボットは、各関節を動作させるとロボット全体の姿勢が変化する。そ の結果、手先効果器以外の箇所でロボットが他の物体と予想外に接触する可能性が ある。接触によって発生する外力は低減できる方が好ましい。そこで、ロボットの先端 リンクに力センサを取り付け、負荷推定手段が、駆動軸に作用する各関節の総負荷 力も力センサが検出する外力による負荷を差し引くことによって、その関節の負荷の 大きさと方向を推定することが好ましい。力センサは例えば、先端リンクと手先効果器 の間に取り付けられる。そのような力センサによって、手先効果器を介してロボットに 加わる外力を検出することができる。各関節軸に作用する負荷は、前述したように力 センサが検出する外力からヤコビ行列によって推定することができる。一方、関節の 駆動軸に作用する総負荷は、前述したようにァクチユエータであるモータへの電流指 令値にトルク定数を乗じることで推定できる。

関節の駆動軸に作用する総負荷力 力センサが検出する外力による負荷を差し引 いた負荷の大きさが閾値を越える過負荷関節については、第 1駆動量算出部によつ て、負荷の作用する方向と同じ方向へ過負荷関節を駆動する目標駆動量が算出さ れる。その目標駆動量に従って過負荷関節が駆動されることによって、ロボットは、接 触状態を解消する方向に姿勢を変化させることができる。同時に、第 2駆動量算出部 によって、非過負荷関節の目標駆動量として、できるだけ先端リンクを目標状態べタト ルに追従させるための駆動量が算出される。そのように算出された目標駆動量に従 つて非過負荷関節を制御することで、目標状態ベクトルの要素のうち、優先順位が高 い要素については追従性が確保される。以上により、ロボットがその先端リンク以外で 物体と接触したとき、その接触状態を解消しながら、先端リンクをできるだけ目標状態 ベクトルに追従させるように動作する多関節ロボットを実現することができる。

[0015] コントローラが有する負荷推定部、過負荷関節特定部、第 1駆動量算出部、第 2駆 動量算出部、及び、ドライバが実行する処理は、コントローラに実装されたコンビユー タプログラムに記述される場合がある。従って、本発明は、多関節ロボットの制御用コ ンピュータプログラムプロダクトに具現ィ匕することもできる。そのコンピュータプログラム プロダクトには、多関節ロボットに以下のステップを実行させる指示を含む。

•各関節の駆動軸に作用している負荷の大きさと方向を推定する負荷推定ステップ； •推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾 値以下の非過負荷関節を特定する過負荷関節特定ステップ；

•負荷の作用する方向と同じ方向に過負荷関節を駆動する過負荷関節の目標駆動 量を算出する第 1駆動量算出ステップ；

•与えられた先端リンクの目標状態ベクトルの要素の中から、与えられた優先順位の 高い順に非過負荷関節と同数の要素を抽出し、抽出した要素の値を実現するように 非過負荷関節を駆動する非過負荷関節の目標駆動量を算出する第 2駆動量算出ス テツプ；

•第 1駆動量算出手段と第 2駆動量算出手段が算出した目標駆動量を、ァクチユエ一 タを制御するドライバへ出力する出力ステップ。

発明の効果

[0016] 本発明によれば、ある関節が過負荷となった場合に、その過負荷状態を解消しな がら、ロボットの先端リンクを目標状態ベクトルにできるだけ追従する多関節ロボットを 実現できる。この多関節ロボットによれば、ある関節が過負荷となった場合に、目標状 態ベクトルに対する先端リンクの状態の追従性は、目標状態ベクトルの要素のうち予 め決められた優先順位の高い順力 確保される。従って、どの関節が過負荷となって も、多関節ロボットが行う作業において、目標状態ベクトルの中の重要な要素につい ては先端リンクの追従性 (先端リンクの現在状態ベクトルの要素の、目標状態ベクトル の要素への追従性）が確保できる。その結果、多関節ロボットは、どの関節が過負荷 となっても可能な限り作業を継続することができる。

図面の簡単な説明 [0017] [図 1]第 1実施例の多関節ロボットの概要図である。

[図 2]第 1実施例の多関節ロボットのブロック図である。

[図 3]第 1実施例の多関節ロボットの制御フローチャート図である。

[図 4]第 2実施例の多関節ロボットのブロック図である。

符号の説明

[0018] 10 :ロボット機構

12aゝ 12bゝ 12cゝ 12d、 12eゝ 12fゝ 12g :リンク

14 :手先効果器

20a, 20b、 20c、 20d、 20e、 22f :関節

22a, 22b, 22c, 22d、 22e、 22f :ァクチユエータ

26 :エンコーダ

28 :力センサ

30 :コントローラ

32 :負荷推定部

34 :過負荷関節特定部

36 :力制御部

38 :モータドライバ

40 :記憶装置

42 :要素抽出部

44 :逆変換部

実施例

[0019] <第 1実施例 > 本発明の第 1実施例に係る多関節ロボットについて、図面を参照 して説明する。図 1は、多関節ロボット 100の概要図を示す。多関節ロボット 100は口 ボット機構 10 (多関節ロボット機構）とコントローラ 30を備える。図 1では、ロボット機構 10を関節とリンクの連結構造を示すスケルトン図で表している。本実施例の多関節口 ボット 100は、初期位置にある水の入ったコップ（不図示）をグリッパ 14で把持し、目 的の位置へ目的の姿勢で位置決めするコップ移動作業を行うロボットである。本実施 例の多関節ロボット 100は、コップ移動作業中に関節に加わる負荷トルクの大きさが 予め設定された閾値を超えた場合に、閾値を超えた関節については負荷を低減する ように制御しながら、残りの関節を用いてコップ移動作業を可能な限り継続することが できる。関節に加わる負荷トルクの大きさが閾値を超える場合とは、例えばロボット機 構 10のある部位が他の物体と接触してロボット機構 10に想定外の外力が加わる場 合に起こり得る。

[0020] ロボット機構 10は、第 1リンク 12a〜第 7リンク 12gの 7個のリンクと、第 1関節 20a〜 第 6関節 20fの 6個の関節を有する。第 1リンク 12aの一端は、床面 94に固定されて いる。即ち、第 1リンク 12aは絶対座標系に対して固定されている。絶対座標系に対し て固定されている第 1リンク 12aは、基部リンクと称されることがある。第 1リンク 12aと 第 2リンク 12bは、第 1関節 20aによって回転可能に連結されている。第 2リンク 12bと 第 3リンク 12cは、第 2関節 20bによって回転可能に連結されている。第 3リンク 12c、 第 4リンク 12d、第 5リンク 12e、第 6リンク 12f、第 7リンク 12gについても同様である。 図 1において、第 1関節 20aのように矩形で表されている関節は、回転軸カ^ンタの長 手方向に沿って伸びている関節を表す。第 1関節 20aでは、第 2リンク 12bは第 1リン ク 12aに対して図 1の矢印 90の方向に回転することができる。また、第 2関節 20bのよ うに円で表されて!/、る関節は、回転軸が図面に垂直な方向に沿つて伸びて 、る関節 を表す。第 2関節 20bでは、第 3リンク 12cは第 2リンク 12bに対して図 1の矢印 92の 方向に回転することができる。

第 1関節 20aにはモータ 22aが取り付けられている。モータ 22aを駆動することによ つて、第 1リンク 12aと第 2リンク 12bの相対角を変えることができる。即ち、モータ 22a によって第 1関節 20aを駆動することができる。第 2関節 20b〜第 6関節 20fにも夫々 モータ 22b〜22fが取り付けられている。以下では、全てのリンクを総称する場合には 、英字の添え字を省略してリンク 12と表し、全ての関節を総称する場合には関節 20 と表し、全てのモータを総称する場合にはモータ 22と表す。

第 7リンク 12gは、ロボット機構 10の先端リンクと称されることがある。第 7リンク 12gに は手先効果器 14が取り付けられている。手先効果器 14はグリッパであり、コップなど の物体を把持することができる。

[0021] ロボット機構 10は、第 1関節 20aから第 6関節 20fまでの 6個の関節を有する 6自由 度ロボットである。従って、多関節ロボット 100は、その先端リンクである第 7リンク 12g (即ち、手先効果器 14)を 3次元空間内で直交 3軸方向の位置（図 1に示す座標系で X座標値、 y座標値、 z座標値）と 3軸周りの回転角（図 1に示す座標系でロール角、ピ ツチ角、ョ一角）の計 6自由度を任意に制御することができる。換言すれば、多関節口 ボット 100は、 6自由度多関節ロボットである。

[0022] 関節 20の夫々にはエンコーダ（不図示）が取り付けられて 、る。エンコーダによって 、各関節の関節角を検出することができる。

コントローラ 30には、エンコーダ (不図示）の検出信号が入力される。またコントロー ラ 30からは各モータ 22へ制御信号 (電流指令値)が出力される。コントローラがモー タ 20を制御することによって、各関節が駆動され、手先効果器 14を目的の位置へ目 的の姿勢で位置決めすることができる。即ち、手先効果器 14が把持するコップを目 的の位置へ目的の姿勢で位置決めすることができる。

[0023] 図 2に、多関節ロボット 100のブロック図を示す。コントローラ 30には、負荷推定部 3 2、過負荷関節特定部 34、力制御演算部 36、モータドライバ 38、記憶装置 40、要素 抽出部 42、及び逆変換部 44が備えられている。記憶装置 40を除く各部は、コント口 ーラ 30が備える CPU (不図示）が実行するプログラムモジュールとしてコントローラ 30 に実装されている。ロボット機構 10には、ァクチユエータ 22とエンコーダ 26が備えら れている。ァクチユエータ 22とエンコーダ 26は、夫々の関節に備えられている。ェン コーダ 26は、各関節の関節角（現在関節角）を検出する。なお、図 2では手先効果器 14の図示を省略している。

[0024] 記憶装置 40には、第 7リンク 12g (先端リンク)の目標状態ベクトルと、優先順位と、 関節の負荷閾値のデータが記憶されている。本実施例における目標状態ベクトルは 、第 7リンク 12gに固定された座標系の絶対座標系に対する相対位置と相対姿勢角 を要素に持つベクトルである。第 7リンク 12gに固定された座標系の絶対座標系に対 する相対位置を目標位置と称し、相対姿勢角を目標姿勢角と称する。具体的には図 1に示す直交 3軸の XYZ座標系において、各座標軸方向の目標位置 (X座標値、 y 座標値、及び z座標値)と、各座標軸周りの目標姿勢角（ロール角、ピッチ角、及びョ 一角）の計 6個の値を要素とするベクトルである。目標状態ベクトルは、経時的に変化 する時系列データとして記憶されている。なお、本明細書では、目標状態ベクトルの 各要素の値を「目標要素」或いは単に目標値と称することがある。また、エンコーダ 2 6の検出値力 算出される第 7リンク 12gに固定された座標系の絶対座標系に対する X座標値、 y座標値、 z座標値、ロール角、ピッチ角、及びョ一角を要素に持つベタト ルを先端リンクの現在状態ベクトルと称する。現在状態ベクトルの個々の要素の値を 現在値と称することがある。

前述したように、ロボット機構 10は 6個の関節を有するので、コントローラ 30が 6個の 関節の関節角を適宜制御することによって、先端リンクの現在状態ベクトルの各要素 の値 (現在値)を目標状態ベクトルの対応する各要素の値に追従させることができる。 即ち、水の入ったコップを目標状態ベクトルに沿って移動させるコップ移動作業が実 行できる。なお、現在状態ベクトルは、エンコーダ 26が検出する各関節角の値から、 V、わゆる順変換によって求めることができる。

記憶装置 40に記憶された優先順位は、目標状態ベクトルの 6個の要素に対して予 め設定されている。ここでいう優先順位とは、コントローラ 30が先端リンクの現在状態 ベクトルを目標状態ベクトルに追従させる制御を行なうときに、現在状態ベクトルの全 ての要素の値を目標状態ベクトルの対応する要素の値に追従させることができな ヽ 場合に、優先的に追従させる要素を指定するものである。本実施例では、水の入つ たコップを移動させる作業であるので、コップの水をこぼさないことが重要である。従 つて本実施例では、図 1に示す座標系において、ロール角、ピッチ角、 z座標値、 X座 標値、 y座標値、ョ一角の順に優先順位が設定されている。ここでロール角とピッチ角 の優先順位が高いのは、これらの角度が目標値からはずれるとコップが水平軸に対 して傾くため、コップ内の水がこぼれる可能性がある力もである。一方、ョ一角が最も 低 、優先順位に設定されて 、るのは、 Z軸 (即ち鉛直方向に沿った軸)周りにコップ が回転してもコップの水はこぼれることは無ぐまたョ一角が変化しても、コップ (先端 リンク）の X座標値、 y座標値、及び z座標値は変わらないからである。 3軸方向の位置 に関して z座標値が X座標値と y座標値よりも優先順位が高 、のは、本実施例の作業 力 コップを初期位置力 持ち上げる 移動する コップを下ろす、という作業である ため、コップの水平方向の位置がずれてもコップの垂直方向の位置が目標値力 ず れないようにするためである。上記の通り優先順位を設定することによって、コップ移 動作業中に最も優先順位の低いョ一角の現在値が目標値に追従できなくなっても、 その他の現在値が目標値に追従すれば、コップ移動作業を続行することができる。ま た、ョ一角の次に優先順位の低い y座標値の現在値が目標値に追従できなくなって も、ョ一角と y座標値以外の現在値が目標値に追従すれば、水をこぼすことなぐコッ プを少なくとも X座標値と z座標値については目標位置に移動させることができる。即 ち、いくつかの関節が過負荷となり、目標状態ベクトルへの追従制御に利用できなく なっても、予定された作業を可能な限り継続することができる。

[0026] ここで、現在状態ベクトルの全ての要素の値を目標状態ベクトルの対応する要素の 値に追従させることができない場合とは、後述するように、関節に作用する負荷の大 きさが閾値を超えた場合にその関節は負荷を低減するように制御されるために目標 状態ベクトルへの追従制御に利用できなくなる場合である。目標状態ベクトルへの追 従制御に利用できる関節の数が 6未満となると、先端リンクの現在状態ベクトルの 6個 の要素を夫々独立に制御することができなくなるからである。

[0027] 記憶装置 40には、関節ごとに夫々の関節が許容できる負荷の大きさが閾値として 記憶されている。閾値は、モータがオーバヒートしないための条件や、或いは、ロボッ トが他の物体に接触した場合にロボット又は他の物体にダメージを与えないための条 件など力 予め設定される。

[0028] 次に、コントローラ 30が実行する制御について説明する。まず、通常の状態 (即ち 全ての関節において、作用する負荷の大きさが閾値を下回っている状態）における 制御について説明する。記憶装置 40に記憶された目標状態ベクトルが所定周期毎 に逆変換部 44へ入力される。なお、要素抽出部 42が行なう処理については後述す る。逆変換部 44にはまた、ロボット機構 10の各関節に取り付けられたエンコーダ 26 が計測する関節角（現在関節角）が入力される。逆変換部 44は、現在関節角を用い て、目標状態ベクトルを各関節の目標関節角（目標関節角ベクトル)へ変換する逆変 換を行う。逆変換式は、ロボット機構 10のリンク構造が既知であり、各制御時における 各関節の現在関節角が取得できれば求めることができる。本実施例では、多関節口 ボットの関節の総数が 6であり、目標状態ベクトルの総要素数も 6であるので、任意の 目標状態ベクトルを実現する目標関節角を求める逆変換式が決定できる。逆変換部

44によって求められた 6個の目標関節角はモータドライバ 38に送られる。モータドラ ィバ 38は、関節ごとに現在関節角が目標関節角に追従するようにァクチユエータ 22 を制御する。以上の制御は従来の多関節ロボットの制御と同じである。

[0029] 次に、関節に作用する負荷の大きさが閾値よりも大きい場合の制御について説明 する。関節に作用する負荷の大きさと方向は負荷推定部 32によって推定される。本 実施例では、モータドライバ 38の最下層の制御ループは電流制御ループである。ァ クチユエータであるモータの出力トルクは、モータに印加される電流値にほぼ比例す る。従って、モータドライバ 38がァクチユエータ 22に出力する電流指令値をモニタす ることによって、ァクチユエータの出力トルクの大きさと方向を推定することができる。 ァクチユエータの出力トルクと、関節軸が出力するトルクの関係は予め調べておくこと ができる。負荷推定部 32は、モータドライバ 38がァクチユエータへ出力する電流指 令値をモニタすることによって、各関節のトルク、即ち、各関節に作用する負荷トルク の大きさと方向を推定する。

[0030] 負荷推定部 32によって推定された各関節の推定負荷の大きさと方向は、過負荷関 節特定部 34へ送られる。過負荷関節特定部 34は、記憶装置 40から各関節の閾値 を読み出し、関節ごとに推定負荷の大きさと比較する。ここで、推定負荷の大きさが 閾値より大きい場合にその関節を過負荷関節と称する。また、過負荷関節でない関 節を非過負荷関節と称する。なお、ここでいう「過負荷関節」とは、推定負荷の大きさ が予め設定された閾値を超えた関節を意味し、推定負荷の大きさが関節の物理的な 許容負荷の大きさを超える関節に限らないことに留意されたい。過負荷関節特定部 3 4によって、ロボット機構 10のすベての関節は、過負荷関節又は非過負荷関節のい ずれかに特定される。

過負荷関節については力制御演算部 36によって、その目標関節角が決定される。 非過負荷関節にっ 、ては逆変換部 44によってその目標関節角が決定される。

[0031] 力制御演算部 36では、過負荷関節について、負荷が作用する方向と同じ方向に 関節を駆動する目標関節角が決定される。本実施例では、力制御ロジックは次の（1 )式で表される。 τ =Κ · ( θ ά - 0 c) · ' · ( 1)

ここで、 τは負荷推定部 32で推定された負荷である。 Κはパネ定数であり、予め設 定されている。 Θ cはエンコーダ 26によって検出された関節角（現在関節角）である。 0 dは目標関節角である。（1)式から目標関節角 0 dは次の（2)式で決定される。

0 d = 0 c + τ /Κ · · · (2)

(2)式の意味は次の通りである。目標関節角 Θ dは、検出された現在の関節角 Θ c に対して、負荷 τが作用する方向に τ ΖΚだけ回転させる位置に決定される。換言 すれば、目標関節角 Θ dは、推定負荷てが関節駆動軸に作用する方向と同じ方向 に関節を駆動するように決定される。ある関節の駆動軸に作用する負荷の大きさが口 ボット機構と他の物体との接触により増大した場合には、その関節を推定負荷てが作 用する方向と同じ方向に回転させる目標関節角を設定し、現在関節角を目標関節角 に追従するように制御することによって、関節に作用する負荷を低減することができる 。推定負荷の大きさが閾値を超える関節 (即ち、過負荷関節)の全てについて（1)式 及び（2)式により目標関節角が決定される。（1 )式と（2)式に表れる変数（τ、Κ、 Θ d、及び Θ c)は関節ごとに異なる値であってよい。

一方、過負荷関節特定部 34の処理結果 (即ち、どの関節が過負荷関節であるかの 情報）は要素抽出部 42と逆変換部 44に送られる。より具体的には、要素抽出部 42 には、過負荷関節の数を示すデータが送られる。過負荷関節の数を示すデータは、 非過負荷関節の数を示すデータと等価である。逆変換部 44には、多関節ロボットが 有する関節の中で過負荷関節を特定するデータが送られる。要素抽出部 42はまた、 目標状態ベクトルと優先順位のデータを記憶装置 40から読み出す。要素抽出部 42 では、目標状態ベクトルの要素の中から、優先順位の高い順に非過負荷関節の数と 同数の要素を抽出する。要素抽出部 42は、抽出した目標要素を逆変換部 44へ送る 。逆変換部 44には過負荷特定部 34から過負荷関節を特定するデータが送られるた め、逆変換部 44では、非過負荷関節を特定することができる。ここで、逆変換部 44 には予め複数の逆変換式 (逆変換式群）が設定されている。その逆変換式群とは、目 標状態ベクトルの要素の任意の組み合わせを入力とし、目標関節角ベクトルの要素 の任意の組み合わせを出力とする逆変換式の集合である。それらの逆変換式群は、 ロボット機構の幾何学的構造から予め求めておくことができる。逆変換部 44では、そ れらの逆変換式群から、要素抽出部 42が抽出した要素を入力とし、非過負荷関節の 目標関節角を出力とする逆変換式を選定する。選定された逆変換式を用いることに よって、要素抽出部 42で抽出された要素を入力としたときの非過負荷関節の目標関 節角を求めることができる。即ち、逆変換部 44で非過負荷関節の目標関節角が決定 される。

[0033] 以上の処理により、過負荷関節の目標関節角は力制御演算部 36で決定され、非 過負荷関節の目標関節角は逆変換部 44で決定される。夫々決定された目標関節角 はモータドライバ 38に送られる。モータドライバ 38では、前述した通常状態 (過負荷 関節がない場合)と同様に、夫々の関節について、現在関節角が決定された目標関 節角に追従するようにァクチユエータ 22が制御される。

[0034] 上記の処理の流れを図 3のフローチャート図を参照して説明する。まず、エンコーダ 26によって各関節の現在の関節角を取得する (ステップ S100)。次に、モータドライ ノ 38からァクチユエータ 22へ送られる電流指令値をモニタすることによって、各関節 に作用する負荷の大きさと方向を推定する (ステップ S102)。次に、関節ごとに推定 した負荷の大きさを閾値と比較する (ステップ S104)。ここで、推定した負荷の大きさ が閾値よりも大きい関節が過負荷関節として特定される。過負荷関節については、 (1 )式、と（2)式により目標関節角が決定される (ステップ S106)。一方、ステップ S104 で過負荷関節が特定されるので、過負荷関節以外の関節が非過負荷関節として特 定される (ステップ S108)。目標状態ベクトルから、優先順位の高い順に非過負荷関 節の数と同数の要素が抽出される (ステップ S110)。抽出された要素を入力とし、非 過負荷関節の目標関節角を出力とする逆変換式が選定される (ステップ S112)。逆 変換式には、ステップ S 100で取得した現在関節角の値が代入される。そうして、逆 変換式の具体的な数値が設定される。設定された逆変換式に抽出された要素の値 を入力して非過負荷関節の目標関節角を決定する (ステップ S114)。ステップ S106 で決定された過負荷関節の目標関節角とステップ S 114で決定された非過負荷関節 の目標関節角がともにモータドライバ 38へ出力される (ステップ S116)。図 3には図 示していないが、モータドライバ 38によって、各関節は現在関節角が目標関節角に 追従するように制御される。以上の処理が制御サンプリング毎に繰り返される。

本実施例の多関節ロボット 100は、 6個の関節を有している。即ち、多関節ロボット 1

00は 6自由度を有している。従って、先端リンクの現在状態ベクトルを 6次元の目標 状態ベクトルに追従するように制御することができる。 6個の関節の全てが非過負荷 関節の場合には、 6個の関節すベてを 6次元の目標状態ベクトルへの追従制御に使 用できる。即ち、手先 (先端リンク)の状態ベクトルを目標状態ベクトルに追従させるこ とができる。ここで、ある関節の推定負荷の大きさが閾値を超える場合には、上記（2) 式によりその関節は目標状態ベクトルに関わりなく負荷の作用する方向に駆動される

。即ち、過負荷関節は、負荷が低減される方向に駆動される。

過負荷関節は、目標状態ベクトルに関わり無く負荷に応じて制御されるため、先端 リンクの現在状態ベクトルを目標状態ベクトルに追従させる制御に利用できなくなる。 目標状態ベクトルへの追従制御に利用できる関節 (即ち非過負荷関節)の数は、目 標状態ベクトルの次数より少なくなる。従って、従来の技術では先端リンクの現在状 態ベクトルを与えられた目標状態ベクトルに追従させることはできない。本実施例の 多関節ロボット 100では、目標状態ベクトルの各要素に対して優先順位が予め設定 されている。目標状態ベクトルの要素の中から、優先順位の高い順に非過負荷関節 と同数の要素が抽出される。即ち、追従制御に利用できる関節の数と同数まで目標 状態ベクトルを低次元化する。そうすることによって、先端リンクの現在状態ベクトルを 低次元化された目標状態ベクトルに追従させる目標関節角が決定できる。目標状態 ベクトルを低次元化する際には、予め決められた優先順位によって、多関節ロボット に実行させる作業に応じてより正確に追従させたい要素が優先的に抽出される。本 実施例の多関節ロボットでは、ある関節が過負荷状態となって先端リンクの現在状態 ベクトルを目標状態ベクトルに完全に追従させることが困難となった場合でも、目標 状態ベクトルのうち重要な要素にっ 、ては高!、追従性が維持されるように非過負荷 関節を制御することができる。これにより、ある関節が過負荷状態となってもできるだ け作業を継続することのできる多関節ロボットが実現できる。本実施例の多関節ロボ ットは、どの関節が過負荷となっても、目標状態ベクトルの要素のうち、常に高順位の 要素について追従性が確保される。本実施例の多関節ロボットによれば、どの関節 が過負荷関節となっても、予定されていた作業を可能な限り継続できる。 なお、本実施例では、目標関節角が目標駆動量の一態様に相当する。図 2の負荷 推定部 32が請求項の負荷推定部の一態様に相当する。過負荷関節特定部 34が請 求項の過負荷関節特定部の一態様に相当する。力制御演算部 36が請求項の第 1 駆動量算出部の一態様に相当する。要素抽出部 42と逆変換部 44が請求項の第 2 駆動量算出部の一態様に相当する。

本実施例では、負荷推定部 32、過負荷関節特定部 34、力制御演算部 36、要素抽 出部 42、及び逆変換部 44はコントローラ 30の CPUが実行するコンピュータプロダラ ムモジュールとして実装される。負荷推定部 32等は、夫々専用の処理を行う専用ハ 一ドウエアで実装されてもよい。換言すれば、 CPUが実行するプログラムは、図 3に 示すフローチャートの各ステップを実行させる指示を含んでいる。

[0036] く第 2実施例〉 次に第 2実施例について説明する。図 4に、第 2実施例の多関節 ロボットのブロック図を示す。図 4では、図 2に示す第 1実施例のブロック図の部品と同 じ部品には同じ符号を付してある。図 2と同じ符号を付した部品については説明を省 略する。

第 2実施例の多関節ロボットにおけるロボット機構 10bは、先端リンク（図 1の第 7リン ク 12g)に力センサ 28を備えている。力センサ 28以外は第 1実施例のロボット機構 10 と同じであるので説明を省略する。力センサ 28は、手先効果器 14 (図 1参照）とロボッ ト機構 10bの先端リンクの間に取り付けられている。即ち、力センサ 28は、手先効果 器 14を介してロボット機構 10bに加わる外力を検出する。

[0037] 第 2実施例のコントローラ 30bと第 1実施例のコントローラ 30 (図 2参照）の相違点は 負荷推定部 32である。負荷推定部 32b以外は第 1実施例のコントローラ 30と同じで あるので説明を省略する。

本実施例の多関節ロボットでは、第 1実施例と同様に各関節の駆動軸に作用する 総負荷をモータドラバ 38からァクチユエータ 22へ指令される電流指令値力も推定す る。負荷推定部 32bには、力センサ 28の検出値とエンコーダ 26の検出値 (各関節の 現在の関節角）が入力される。負荷推定部 32bは、力センサによって検出される外力 (即ち手先効果器 14を介してロボット機構 10bに加わる外力）を各関節の駆動軸へ の負荷トルク（手先外力による負荷トルク）に変換する。負荷推定部 32bは推定した 総負荷の値力 手先外力による負荷トルクの値を減じた値を最終的な推定負荷とし て出力する。このときの出力には推定負荷の大きさと方向が含まれる。なお手先効果 器 14を介してロボット機構に加わる外力の各駆動軸への変換は、現在の関節角にか ら算出されるヤコビ行列の転置行列を用いて得られる。即ち、本実施例における負荷 推定部 32bは、関節の駆動軸に作用する総負荷力も力センサが検出する外力（手先 効果器を介してロボット機構 10に作用する外力）による負荷を除いた負荷の大きさと 方向を推定する。以下では、関節の駆動軸に作用する総負荷力 力センサが検出 する外力による負荷を除いた負荷を想定外負荷と称する。想定外負荷の原因となる 外力を想定外外力と称する。想定外外力は、例えば多関節ロボットが作業中にその 姿勢を変えることによって、ロボット機構の手先効果器以外の箇所が他の物体と接触 した場合などに生ずる外力である。即ち、想定外外力に起因する想定外負荷の大き さが閾値を越えた場合には、ロボット機構が他の物体と予期せぬ接触を生じている可 能性が高 、。そのような場合にはロボットは他の物体との接触を回避するように動作 させることが好ましい。本実施例のコントローラ 30bは、負荷推定部 32bを除いて第 1 実施例のコントローラ 30と同様である。従って負荷推定部 32bによって推定された負 荷 (想定外負荷)の大きさが閾値よりも大きい過負荷関節については、負荷の作用す る方向と同じ方向に関節が駆動される。これによつて、ロボットと他の物体との接触を 解消する方向に関節を動作させることができる。なお、本実施例における閾値の値は 、第 1実施例における閾値の値と異なってよい。具体的には、本実施例では負荷推 定部 32bは想定外外力に起因する想定外負荷を推定するので、閾値は想定外負荷 を許容できる値に設定される。

本実施例の多関節ロボットは、手先効果器以外の部位で他の物体と接触した場合 に、その接触を解消しながら、接触の解消に関与しない関節 (即ち非過負荷関節）は 、できるだけ先端リンクの現在状態ベクトルを目標状態ベクトルに追従させるように制 御される。即ち、本実施例の多関節ロボットは、手先効果器以外の部位で他の物体と 接触した場合に、その接触を解消しながら、ロボットが実行中の作業を可能な限り継 続することができる。 [0038] <第 3実施例 > 次に本発明の第 3実施例について説明する。第 1実施例と第 2実 施例では、非過負荷関節の目標関節角を決定する際に逆変換を用いた。第 3実施 例では、各関節を制御する際の目標値（目標駆動量)は関節角速度であり、目標状 態ベクトルは先端リンクの目標速度（図丄の座標系における _x、 y、 z各軸方向の速度 及び、ロール角速度、ピッチ角速度、ョー角速度)で表される。即ち、過負荷関節が 存在しない場合には第 3実施例の多関節ロボットはいわゆる分解速度制御が行なわ れる。本実施例は、非過負荷関節の目標値 (角速度)を決定する際に分解速度制御 を応用する。なお、本実施例では、多関節ロボットのブロック図は図 2を参照する。伹 し図 2における逆変換部 44は、分解速度制御演算部に置き換える。

[0039] 先端リンクの目標状態ベクトルから目標関節角速度ベクトル（関節の目標駆動量べ タトル)へはヤコビ行列を用いて変換できる。目標状態ベクトルを dXで表し、各関節 の現在関節角を要素とする関節角ベクトルを Θで表し、各関節の目標角速度を要素 とする目標関節角速度ベクトル（目標駆動量ベクトル)を で表し、ヤコビ行列を J ( Θ)で表すと、それらの関係は次の（3)式で表せる。

dX=j (0) -d@ · ' · (3)

従って、目標関節角速度ベクトル (1Θは次の (4)式で表せる。

d@ =J^_1 (0) -dX · · · (4)

ここで J—¹ (Θ)はヤコビ行歹 (Θ)の逆行列である。第 1実施例と同じぐ多関節ロボ ットは 6個の関節を有しており、目標状態ベクトルは、先端リンクの X軸、 y軸、 z軸方向 の速度と、 3軸夫々の軸周りのロール角速度、ピッチ角速度、及びョー角速度を要素 とする 6次元ベクトルである。（3)式において、 dXと (1Θが共に 6次元であるのでヤコ ビ行歹 ( Θ )は 6 X 6の正方行列となる。従って、ヤコビ行歹 ( Θ )の行列式がゼロで な 、限り逆行歹 —¹ ( Θ )が存在して (4)式によって目標状態ベクトル dXから (1Θを求め ることができる。各要素が (角）速度の単位を有する目標状態ベクトルに対して、ヤコ ビ行列の逆行列を用いて目標関節角速度を算出し、得られた目標関節角速度を実 現するように各関節のァクチユエータを制御する方法が分解速度制御法と呼ばれる 制御法である。

[0040] 本実施例において、過負荷関節が存在する場合の処理を説明する。今、図 2の過 負荷特定部 34によって、 6次元の関節角ベクトル Θの第 i要素の関節が過負荷関節 と特定されたとする。一つの過負荷関節が存在するので、要素抽出部 42は 6次元の 目標状態ベクトル dXの要素のうち優先順位の上位カゝら 5個の要素を抽出する。換言 すれば、 5次元に低次元化した目標状態ベクトルを抽出する。ここで、 6次元の目標 状態ベクトル dXの要素のうち、第 j要素が最も優先順位の低い要素とする。本実施例 の分解速度制御演算部は、第 j要素の欠けた 5次元の目標状態べ外ル (dX'と表す )から第 i要素の欠けた 5次元の目標関節角速度ベクトル (άΘ 'と表す)を決定する。（ 3)式に示した 6 X 6のヤコビ行歹 ( Θ )力も第 j行と第 i列を除外した 5 X 5の低次元ャ コビ行列を J' (Θ)と表すと、次の（5)式が成立することがわかる。

dX' =j' (Θ) ·άΘ ' · ' · (5)

従って、 5次元の目標関節角速度ベクトル (1Θ 'は次の（6)式で求められる。

άΘ ' =ΐ'^_1 (Θ) ·άΧ' · ' · (6)

本実施例の分解速度制御演算部は、 (6)式によって非過負荷関節 (第潘目の関 節を除く関節)の目標関節角速度ベクトル 'を決定することができる。決定された 目標関節角速度ベクトル 'の各要素 (各非過負荷関節の目標関節角速度）はモ ータドライバ 38に送られる。モータドライバ 38は目標関節角速度に追従するようにァ クチユエータ 22を制御する。なお、本実施例における力制御演算部 36は、駆動軸ご とにインピーダンス制御或いは力制御の制御則に基づいて、過負荷関節の目標関 節角速度を算出する。

なお、上記例では、過負荷関節はひとつと仮定したが、過負荷関節が複数存在す る場合も同様の議論が成立する。

第 3実施例では、分解速度制御法を利用して非過負荷関節の目標関節角速度（目 標駆動量)を算出する。分解速度制御はヤコビ行列の逆行列を用いて速度の単位を 有する要素からなる目標状態ベクトルから各関節の目標関節角速度を決定する方法 である。第 3実施例では、多関節ロボットのヤコビ行列を設定し、そのヤコビ行列にお いて、特定された過負荷関節に対応する列 (上記例では第冽）と、抽出されな力つた 目標状態ベクトルの要素に対応する行 (上記例では第 j行)を除外した低次元ヤコビ 行列を特定する。その低次元ヤコビ行列の逆行列を用いて抽出された目標状態べク トルの要素から非過負荷関節の目標関節角速度（目標駆動量)を決定する。第 3実 施例では、目標状態ベクトルへの追従制御に利用できる関節（非過負荷関節）の数 を特定する。そして目標状態ベクトルについて、その要素の数を非過負荷関節の数 と同数に低次元化する。非過負荷関節の数と目標状態ベクトルの次元を一致させる ことによって、それらの間に成立するヤコビ行列を常に正方行列に維持することがで きる。ヤコビ行列が正方行列に維持されるので、ヤコビ行列の逆行列を算出すること ができる (但し特異点は除く）。従って、先端リンクの現在状態ベクトルを、低次元化し た目標状態ベクトルに追従させる目標駆動量を、低次元化したヤコビ行列によって 簡単に決定することができる。本実施例における分解速度制御演算部（図 2或いは 図 4において逆変換部 44と置き換えられる）と図 2或いは図 4の要素抽出部 42が、請 求項の第 2駆動量算出部の一態様に相当する。

図 3に示したフローチャートの処理は、コントローラ 30、或いは 30bによって実行さ れる。これらの処理は、コントローラ 30が実行するコンピュータプログラムに記述され ている。従って、本発明は、多関節ロボットの制御用コンピュータプログラムプロダクト として具現化されることもある。図 3に示したフローチャートを要約すると、そのコンビュ ータプログラムプロダクトは、以下のステップを実行させる指示を含む。

•各関節の駆動軸に作用している負荷の大きさと方向を推定する負荷推定ステップ（ ステップ S 102)；

•推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾 値以下の非過負荷関節を特定する過負荷関節特定ステップ (ステップ S104) ; '負荷の作用する方向と同じ方向に過負荷関節を駆動する過負荷関節の目標駆動 量を算出する第 1駆動量算出ステップ (ステップ S106)；

•与えられた先端リンクの目標状態ベクトルの要素の中から、与えられた優先順位の 高い順に非過負荷関節と同数の要素を抽出し、抽出した要素の値を実現するように 非過負荷関節を駆動する非過負荷関節の目標駆動量を算出する第 2駆動量算出ス テツプ（ステップ S 110、 S112, S114)；

•第 1駆動量算出手段と第 2駆動量算出手段が算出した目標駆動量を、ァクチユエ一 タを制御するドライバへ出力する出力ステップ (ステップ S 116)。 [0043] 以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の 範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した 具体例を様々に変形、変更したものが含まれる。

[0044] 例えば、各駆動軸にはロボットの自重に起因する負荷も作用する。或いは、ロボット が高速に動作する場合には慣性力に起因する負荷も作用する。ロボットの自重ゃ慣 性力による負荷は予想される負荷であり、過負荷関節を特定する際に閾値と比較す る推定負荷カゝら除外した方がよい場合もある。そのような場合には、負荷推定部は、 関節ごとに、駆動軸に作用する総負荷力 重力に起因する負荷を除外した負荷の大 きさと方向を推定することも好適である。或いは、負荷推定部は、関節ごとに、駆動軸 に作用する総負荷力 慣性力に起因する負荷を除外した負荷の大きさと方向を推定 することも好適である。さらに負荷推定部は、関節ごとに、駆動軸に作用する総負荷 から、重力と慣性力の双方に起因する負荷を除く負荷の大きさと方向を推定すること も好適である。なお、ロボットの自重に起因して駆動軸に作用する負荷の大きさと方 向は、ロボットの各部の質量と慣性モーメントと各関節の現在関節角によって算出で きる。慣性力に起因して駆動軸に作用する負荷の大きさと方向は、さらに各関節の関 節角加速度と角速度によって算出できる。

[0045] 本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによつ て技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるも のではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し 得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つも のである。

Claims

請求の範囲

[1] 多関節ロボット機構とコントローラを有する多関節ロボットであり、

多関節ロボット機構は、

複数のリンクと複数の関節と複数のァクチユエータを有しており、隣接するリンクが 関節を介して連結されており、夫々のァクチユエータが夫々の関節を駆動することに よって先端リンクの状態を変化させることが可能であり、

コントローラは、

先端リンクの状態の目標状態ベクトルと、目標状態ベクトルの要素毎に設定されて

V、る優先順位を記憶して!/、る記憶部と、

各関節の駆動軸に作用している負荷の大きさと方向を推定する負荷推定部と、 推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾 値以下の非過負荷関節を特定する過負荷関節特定部と、

過負荷関節に対して、負荷の作用する方向と同じ方向に過負荷関節を駆動する目 標駆動量を算出する第 1駆動量算出部と、

目標状態ベクトルの要素の中から優先順位の高い順に非過負荷関節と同数の要 素を抽出し、非過負荷関節に対して、抽出した要素の値を実現するように非過負荷 関節を駆動する目標駆動量を算出する第 2駆動量算出部と、

第 1駆動量算出部と第 2駆動量算出部が算出した目標駆動量に基づいてァクチュ エータを制御するドライバを備えることを特徴とする多関節ロボット。

[2] 各ァクチユエータは印加して 、る電流と出力トルクが比例するモータであり、

負荷推定手段は、ドライバがモータへ出力する電流指令値力 負荷の大きさと方向 を推定することを特徴とする請求項 1の多関節ロボット。

[3] 先端リンクに力センサが取り付けられており、負荷推定手段は、力センサが検出す る外力から、各関節の駆動軸に作用している負荷の大きさと方向を推定することを特 徴とする請求項 1の多関節ロボット。

[4] 先端リンクに力センサが取り付けられており、負荷推定手段は、駆動軸に作用する 各関節の総負荷力 力センサが検出する外力による負荷を除くことによって、関節の 負荷の大きさと方向を推定することを特徴とする請求項 1に記載の多関節ロボット。 複数のリンクと複数の関節と複数のァクチユエータを有しており、隣接するリンクが 関節を介して連結されており、夫々のァクチユエータが夫々の関節を駆動することに よって先端リンクの状態を変化させることが可能である多関節ロボットを制御するため のコンピュータプログラムであり、

各関節の駆動軸に作用している負荷の大きさと方向を推定する負荷推定ステップと 推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾 値以下の非過負荷関節を特定する過負荷関節特定ステップと、

負荷の作用する方向と同じ方向に過負荷関節を駆動する過負荷関節の目標駆動 量を算出する第 1駆動量算出ステップと、

与えられた先端リンクの目標状態ベクトルの要素の中から、与えられた優先順位の 高い順に非過負荷関節と同数の要素を抽出し、抽出した要素の値を実現するように 非過負荷関節を駆動する非過負荷関節の目標駆動量を算出する第 2駆動量算出ス テツプと、

第 1駆動量算出手段と第 2駆動量算出手段が算出した目標駆動量を、ァクチユエ ータを制御するドライバへ出力する出力ステップを実行させる指示を含むコンビユー タプログラム。