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

Description

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

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

ｎ自由度の多関節ロボットの先端リンクに対してｎ以下の目標値ベクトルが与えられると、先端リンクを目標値ベクトルに一致させるための各関節の目標駆動量を決定することができる。関節の目標駆動量の単位は、位置（角度）や速度（角速度）など、様々な単位系を取り得る。本明細書における「目標駆動量」との表現は、関節の目標関節角や目標角速度といった、関節の「目標値」を含む意味で用いる。従って、例えば関節の現在関節角を目標関節角に一致するように関節を駆動することを、「関節を目標駆動量に一致させる」と表現する場合がある。
先端リンクを目標値ベクトルに一致させるための各関節の目標駆動量は、いわゆる逆変換やヤコビ行列によって求められることは良く知られている。関節の目標駆動量が求まると、関節の現在値（現在関節角や現在関節角速度など）が関節の目標駆動量（目標関節角や目標関節角速度など）に一致するように、関節を駆動するためのアクチュエータが制御される。ここに「関節を駆動する」とは、関節に連結されたリンク同士の相対位置（姿勢）を変化させることである。回転関節の場合は関節に連結されたリンク間の角度（関節角）を変化させることに相当する。直動関節の場合は関節に連結されたリンクの間の距離を変化させることに相当する。

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

ところで、ロボットの作業中に、関節に作用する負荷が増大する可能性がある。例えば、ロボットが予期せぬ物体に接触したにもかかわらず、時間的に変化する目標値ベクトルに追従する制御を継続すると、結果的にロボットが物体を押しのけることになる。ロボットが物体を押しのける際に関節に作用する負荷が増大する恐れがある。あるいは、ロボットが先端リンクに取り付けられたグリッパによって物体を持ち上げる作業を行う場合に、持ち上げる物体の質量が予想以上に大きいと、関節に作用する負荷も予想以上に増加する。関節に作用する負荷のうち、関節の駆動軸に作用している負荷成分については、その関節を駆動するアクチュエータにトルクリミッタなどの過負荷対策を設けることによって緩和することができる。以下では関節の駆動軸に作用する負荷を対象とする。また、本明細書にいう過負荷とは、関節に作用する負荷が予め設定された閾値を超える場合を意味する。即ち、本明細書にいう過負荷とは、関節の物理的な許容限度を超える負荷が関節に作用する場合に限られない。
過負荷対策として関節に装備されるトルクリミッタにはメカニカルなトルクリミッタとソフトウエア的なトルクリミッタがある。特許文献１には、ソフトウエア的なトルクリミッタの一例が開示されている。特許文献１に開示された技術は、アクチュエータであるモータを流れる電流を監視し、通電電流が所定値を超えた場合にモータに通電する電流の供給を停止するという技術である。

特開２００５−１６１４６９号公報

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

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

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

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

本発明は、多関節ロボットの自由度数は目標値ベクトルの次数以上でなければならないとのこれまでの通念に対して、先端リンクを目標値ベクトルに一致させる制御に利用できる関節の数と同数まで目標値ベクトルを低次元化するという新たな発想に基づく。さらに目標値ベクトルの要素には追従性に関して優先順位があることに着目し、目標値ベクトルの要素に優先順位を予め設定する。設定された優先順位の低い順に切り捨てて目標値ベクトルを低次元化することによって、作業にとって重要な目標値要素の追従性を維持する。一方、過負荷関節については、目標値ベクトルに関わらず、過負荷状態を緩和するように関節の目標駆動量を設定する。
過負荷関節の過負荷状態を緩和しながら当初に予定されていた作業をできるだけ継続し得る多関節ロボットを実現することに成功した。
なお、上記の議論は目標値ベクトルが与えられたとき、その単位系に依存しない。即ち、目標値ベクトルの要素は位置や角度（姿勢角）だけでなくそれらの速度や加速度であってもよい。このことは関節の目標駆動量が角度の単位に限られず角速度や角加速度やトルクの単位であっても成立することを意味する。或いは、関節が直動関節の場合には、関節の目標駆動量が伸展長さ、伸展速さ、伸展加速度、伸展させる力であっても成立する。

本発明は、多関節ロボット本体とコントローラを有する多関節ロボットに具現化できる。多関節ロボット本体は、複数のリンクと複数の関節と複数のアクチュエータを有しており、隣接するリンクが関節を介して連結されており、夫々のアクチュエータが夫々の関節を駆動することによって基部リンクに対する先端リンクの位置を変化させることが可能に構成されている。
コントローラは、記憶部、負荷推定部、過負荷関節特定部、第１駆動量算出部、第２駆動量算出部、及びドライバを備える。
記憶部には先端リンクの目標値ベクトルと、目標値ベクトルの要素毎に設定されている優先順位が記憶されている。負荷推定部は、各関節の駆動軸に作用している負荷の大きさと方向を推定する。過負荷関節特定部は、推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾値以下の非過負荷関節を特定する。第１駆動量算出部は、過負荷関節に対して、負荷の作用する方向と同じ方向に過負荷関節を駆動する目標駆動量を算出する。一方、第２駆動量算出部は、目標値ベクトルから優先順位の高い順に非過負荷関節と同数の目標値要素を抽出し、非過負荷関節に対して、抽出した目標値要素を実現するように非過負荷関節を駆動する目標駆動量を算出する。そしてドライバは、第１駆動量算出部と第２駆動量算出部が算出した目標駆動量に基づいてアクチュエータを制御する。
各関節の駆動軸に加わる負荷とは、回転関節の場合は駆動軸に加わるトルクであり、直動関節の場合は駆動軸の伸びる方向に加わる力である。
負荷推定部は、回転関節の場合には関節軸に取り付けられたトルクセンサからの信号を受信し、受信した信号に所定の係数を乗じて負荷の大きさと方向を推定する演算部でよい。或いは、アクチュエータが電流駆動のモータの場合には、ドライバがモータへ出力する電流指令値からアクチュエータの出力トルクの大きさと方向を推定し、その出力トルクから駆動軸に加わる負荷の大きさと方向を推定する推定器でもよい。電流駆動のモータは、通電する電流値と出力トルクがほぼ比例関係を有する場合が多い。そのようなモータの場合は、ドライバがモータへ出力する電流値にいわゆるトルク定数を乗じることで、モータの出力トルクを推定できる。モータの出力トルクに、モータ軸から関節駆動軸へのトルク伝達定数を乗じることによって、関節駆動軸の出力トルクを推定できる。関節駆動軸の出力トルクの大きさを負荷の大きさの推定値としてよい。出力トルクの方向に対して逆方向を負荷の方向の推定値としてよい。或いは、先端リンクに力センサを取り付け、力センサが検出する外力の大きさと方向から各関節の駆動軸に作用する負荷の大きさと方向を推定してもよい。力センサが検出する外力から各関節の駆動軸に作用する負荷への変換は、良く知られているようにヤコビ行列を用いて算出できる。
関節の目標駆動量は前述したように、回転関節の場合は関節角、関節角速度、関節角加速度、あるいはトルクのいずれかであってよい。直動関節の場合は、伸展長さ、伸展速さ、伸展加速度、或いは伸展させる力のいずれかであってよい。また、目標値ベクトルは経時的に変化するものであってもよい。また、閾値は関節ごとに異なる値でよいし、経時的に変化するものであってもよい。
また、推定した負荷の大きさが閾値よりも大きい関節を過負荷関節と特定し、推定した負荷の大きさが閾値以下である関節を非過負荷関節と特定する。ここで、「閾値よりも大きい」と「閾値以下である」との分類を、「閾値以上である」と「閾値未満である」との分類に変更しても本発明の技術的範囲を超えるものではないことを明記する。本発明では、閾値を設けることは重要であるが、閾値と同じ負荷である関節を非過負荷関節に分類するのは単に数学的に矛盾なく閾値の上下で関節を分類するための便宜にすぎない。閾値と同じ負荷である関節を過負荷関節に分類しても本発明の特徴はなんら変更されるものではない。

上記構成では、過負荷関節特定部が、全ての関節を閾値を越える負荷が加わっている過負荷関節とそうでない非過負荷関節に分ける。過負荷関節については、第１駆動量算出部によって、目標ベクトルに関わらずに負荷と同じ方向に過負荷関節を駆動する目標駆動量が決定される。過負荷関節を負荷が作用する方向と同じ方向に駆動すれば、負荷を低減できる可能性が高いからである。一方、過負荷となっておらず目標値ベクトルへの追従制御に利用できる非過負荷関節については、第２駆動量算出部によって、可能な範囲で先端リンクを目標値ベクトルに追従させるように非過負荷関節を駆動する目標駆動量を決定する。具体的には、非過負荷関節の数と同数の要素を目標値ベクトルから抽出する。非過負荷関節の数と抽出した要素数が一致するので、抽出した要素の目標値を実現することのできる目標駆動量が決定できる。さらに、目標値ベクトルの要素は、予め設定された優先順位の高い順に抽出される。即ち、どの関節が過負荷となっても、追従性が重要視される要素が常に抽出され、その要素については現在値の目標値への追従性が確保される。即ち、予定されていた作業の動作に近い動作が実現できる。
各関節の目標駆動量の決定は、目標駆動量が関節角で表される場合には逆変換（逆運動学）により決定することができる。或いは順変換の繰り返し計算によっても決定することができる。また目標駆動量が関節角速度で表される場合にはいわゆる分解速度制御法に基づいて決定することができる。

ロボットに作業を行わせる際には、ロボットの先端リンクに手先効果器が取り付けられている場合が多い。手先効果器はグリッパ（多指ハンドを含む）、切削ツール、溶接ツールなど、作業目的に応じて選択される。そのような場合、手先効果器は作業対象物（例えば前述したコップ移動作業におけるコップなど）と接触するために、手先効果器を介してロボットに外力が加わる。そのような外力は予定されている外力である。その一方で、多関節ロボットは、各関節を動作させるとロボット全体の姿勢が変化する。その結果、手先効果器以外の箇所でロボットが他の物体と予想外に接触する可能性がある。接触によって発生する外力は低減できる方が好ましい。そこでロボットの先端リンクに力センサを取り付け、負荷推定手段は、関節の駆動軸に作用する総負荷から力センサが検出する外力による負荷を除いた負荷の大きさと方向を推定することが好ましい。力センサは例えば、先端リンクと手先効果器の間に取り付けられる。これによって、手先効果器を介してロボットに加わる外力を力センサによって検出することができる。力センサが検出する外力によって各関節軸に作用する負荷は前述したようにヤコビ行列により推定することができる。一方、関節の駆動軸に作用する総負荷は、前述したようにアクチュエータであるモータへの電流指令値にトルク定数を乗じることで推定できる。
関節の駆動軸に作用する総負荷から力センサが検出する外力による負荷を除いた負荷の大きさが閾値を越える過負荷関節については、第１駆動量算出部によって、負荷の作用する方向と同じ方向へ過負荷関節を駆動する目標駆動量が算出される。その目標駆動量に基づいて過負荷関節が駆動されることによって、ロボットは接触状態を解消する方向に姿勢を変化させることができる。同時に、第２駆動量算出部によって、非過負荷関節の目標駆動量として、できるだけ先端リンクを目標値ベクトルに一致させるための駆動量が算出される。そのように算出された目標駆動量に基づいて非過負荷関節が制御されることで、目標値ベクトルの要素のうち、優先順位が高い要素については追従性が確保される。以上により、ロボットがその先端以外で物体と接触したとき、その接触状態を解消しながら、先端リンクをできるだけ目標値ベクトルに一致させるように動作する多関節ロボットを実現することができる。

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

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

ロボット本体１０は、第１リンク１２ａ〜第７リンク１２ｇの７個のリンクと、第１関節２０ａ〜第６関節２０ｆの６個の関節を有する。第１リンク１２ａの一端は、床面９４に固定されている。即ち、第１リンク１２ａは絶対座標系に対して固定されている。絶対座標系に対して固定されている第１リンク１２ａは、基部リンクと称されることがある。第１リンク１２ａと第２リンク１２ｂは、第１関節２０ａによって回転可能に連結されている。第２リンク１２ｂと第３リンク１２ｃは、第２関節２０ｂによって回転可能に連結されている。第３リンク１２ｃ、第４リンク１２ｄ、第５リンク１２ｅ、第６リンク１２ｆ、第７リンク１２ｇについても同様である。図１において、第１関節２０ａのように矩形で表されている関節は、接続されているリンクの長手方向を回転軸とする関節を表す。第１関節２０ａでは、第２リンク１２ｂは第１リンク１２ａに対して図１の矢印９０の方向に回転することができる。また、第２関節２０ｂのように円で表されている関節は、図面に垂直な方向を軸とする関節を表す。第２関節２０ｂでは、第３リンク１２ｃは第２リンク１２ｂに対して図１の矢印９２の方向に回転することができる。
第１関節２０ａにはモータ２２ａが取り付けられている。モータ２２ａを駆動することによって、第１リンク１２ａと第２リンク１２ｂの相対角を変えることができる。即ち、モータ２２ａによって第１関節２０ａを駆動することができる。第２関節２０ｂ〜第６関節２０ｆにも夫々モータ２２ｂ〜２２ｆが取り付けられている。以下では、全てのリンクを総称する場合にはリンク１２と表し、全ての関節を総称する場合には関節２０と表し、全てのモータを総称する場合にはモータ２２と表す。
第７リンク１２ｇはロボット本体１０の先端リンクと称されることがある。第７リンク１２ｇには手先効果器１４が取り付けられている。手先効果器１４はグリッパであり、コップなどの物体を把持することができる。

ロボット本体１０は、第１関節２０ａから第６関節２０ｆまでの６個の関節を有する６自由度ロボットである。従って、多関節ロボット１００は、その先端リンクである第７リンク１２ｇ（即ち、手先効果器１４）を３次元空間内で直交３軸方向の位置（図１に示す座標系でｘ座標値、ｙ座標値、ｚ座標値）と３軸周りの回転角（図１に示す座標系でロール角、ピッチ角、ヨー角）の計６自由度を任意に制御することができる。換言すれば、多関節ロボット１００は、６自由度多関節ロボットである。

関節２０の夫々にはエンコーダ（不図示）が取り付けられている。エンコーダによって、各関節の関節角を検出することができる。
コントローラ３０には、エンコーダ（不図示）の検出信号が入力される。またコントローラ３０からは各モータ２２へ制御信号（電流指令値）が出力される。コントローラがモータ２０を制御することによって、各関節が駆動され、手先効果器１４を目的の位置へ目的の姿勢で位置決めすることができる。即ち、手先効果器１４が把持するコップを目的の位置へ目的の姿勢で位置決めすることができる。

図２に多関節ロボット１００のブロック図を示す。コントローラ３０には、負荷推定部３２、過負荷関節特定部３４、力制御演算部３６、モータドライバ３８、記憶装置４０、目標値要素抽出部４２、及び逆変換部４４が備えられている。記憶装置４０を除く各部は、コントローラ３０が備えるＣＰＵ（不図示）が実行するプログラムモジュールとしてコントローラ３０に実装されている。ロボット本体１０には、アクチュエータ２２とエンコーダ２６が備えられている。アクチュエータ２２とエンコーダ２６は、夫々の関節に備えられている。エンコーダ２６は、各関節の関節角（現在関節角）を検出する。なお、図２では手先効果器１４の図示を省略している。

記憶装置４０には、第７リンク１２ｇ（先端リンク）の目標値ベクトルと優先順位と負荷閾値のデータが記憶されている。本実施例における目標位置ベクトルは、第７リンク１２ｇに固定された座標系の絶対座標系に対する相対位置と相対姿勢角を要素に持つベクトルである。第７リンク１２ｇに固定された座標系の絶対座標系に対する相対位置を目標位置と称し、相対姿勢角を目標姿勢角と称する。具体的には図１に示す直交３軸のＸＹＺ座標系において、各座標軸方向の目標位置（ｘ座標値、ｙ座標値、及びｚ座標値）と、各座標軸周りの目標姿勢角（ロール角、ピッチ角、及びヨー角）の計６個の要素を有するベクトルである。目標値ベクトルは、経時的に変化する時系列データとして記憶されている。なお、本明細書では、目標値ベクトルの要素を単に目標値と称することがある。また、エンコーダ２６の検出値から算出される第７リンク１２ｇに固定された座標系の絶対座標系に対するｘ座標値、ｙ座標値、ｚ座標値、ロール角、ピッチ角、及びヨー角を要素に持つベクトルを先端リンクの現在値ベクトルと称する。現在値ベクトルの個々の要素を単に現在値と称することがある。
前述したようにロボット本体１０は６個の関節を有するので、コントローラ３０が６個の関節の夫々の関節角を適宜制御することによって、先端リンクの現在値ベクトルの各要素を目標値ベクトルの対応する各要素に追従（一致）させることができる。即ち、水の入ったコップを目標値ベクトルに沿って移動させるコップ移動作業が実行できる。なお、現在値ベクトルは、エンコーダ２６が検出する各関節角の値から、いわゆる順変換によって求めることができる。

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

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

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

次にコントローラ３０が実行する制御について説明する。まず、通常の状態（即ち全ての関節において作用する負荷の大きさが閾値を下回っている状態）における制御について説明する。記憶装置４０に記憶された目標値ベクトルが所定周期毎に逆変換部４４へ入力される。なお、目標値要素抽出部４２が行なう処理については後述する。逆変換部４４にはまた、ロボット本体１０の各関節に取り付けられたエンコーダ２６が計測する関節角（現在関節角）が入力される。逆変換部４４は、現在関節角を用いて、目標値ベクトルを各関節の目標関節角（目標関節角ベクトル）へ変換する逆変換を行う。逆変換式は、ロボット本体１０のリンク構造が既知であり、各制御時における各関節の現在関節角が取得できれば求めることができる。本実施例では、多関節ロボットの関節の総数が６であり、目標値ベクトルの総要素数も６であるので、任意の目標値ベクトルを実現する目標関節角を求める逆変換式が設定される。逆変換部４４によって求められた６個の目標関節角はモータドライバ３８に送られる。モータドライバ３８は、関節ごとに現在関節角が目標関節角に追従するようにアクチュエータ２２を制御する。以上の制御は従来の多関節ロボットの制御と同じである。

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

負荷推定部３２によって推定された各関節の推定負荷の大きさと方向は過負荷関節特定部３４へ送られる。過負荷関節特定部３４は、記憶装置４０から各関節の閾値を読み出し、関節ごとに推定負荷の大きさと比較する。ここで、推定負荷の大きさが閾値より大きい場合にその関節を過負荷関節と称する。また、過負荷関節でない関節を非過負荷関節と称する。なお、ここでいう「過負荷関節」とは、推定負荷の大きさが予め設定された閾値を超えた関節を意味し、推定負荷の大きさが関節の物理的な許容負荷の大きさを超える関節に限らないことに留意されたい。過負荷関節特定部３４によって、多関節ロボット本体１０のすべての関節は、過負荷関節か非過負荷関節のどちらかに特定される。
過負荷関節については力制御演算部３６によって、その目標関節角が決定される。非過負荷関節については逆変換部４４によってその目標関節角が決定される。

力制御演算部３６では、過負荷関節について、負荷が作用する方向と同じ方向に関節を駆動する目標関節角が決定される。本実施例では、力制御ロジックは次の（１）式で表される。
τ＝Ｋ・（θｄ−θｃ） ・・・（１）
ここで、τは負荷推定部３２で推定された負荷である。Ｋはバネ定数であり、予め設定されている。θｃはエンコーダ２６によって検出された関節角（現在関節角）である。θｄは目標関節角である。（１）式から目標関節角θｄは次の（２）式で決定される。
θｄ＝θｃ＋τ／Ｋ ・・・（２）
（２）式の意味は次の通りである。目標関節角θｄは、検出された現在の関節角θｃに対して、負荷τが作用する方向にτ／Ｋだけ回転させる位置に決定される。換言すれば、目標関節角θｄは、推定負荷τが関節駆動軸に作用する方向と同じ方向に関節を駆動するように決定される。ある関節の駆動軸に作用する負荷の大きさがロボット本体と他の物体との接触により増大した場合には、その関節を推定負荷τが作用する方向と同じ方向に回転させる目標関節角を設定し、現在関節角を目標関節角に追従するように制御することによって、関節に作用する負荷の大きさを低減することができる。推定負荷の大きさが閾値を超える関節（即ち、過負荷関節）の全てについて（１）式及び（２）式により目標関節角が決定される。（１）式と（２）式に表れる変数（τ、Ｋ、θｄ、及びθｃ）は関節ごとに異なる値であってよい。

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

以上の処理により、過負荷関節の目標関節角は力制御演算部３６で決定され、非過負荷関節の目標関節角は逆変換部４４で決定される。夫々決定された目標関節角はモータドライバ３８に送られる。モータドライバ３８では、前述した通常状態（過負荷関節がない場合）と同様に、夫々の関節について、現在関節角が決定された目標関節角に追従するようにアクチュエータ２２が制御される。

上記の処理の流れを図３のフローチャート図を参照して説明する。まず、エンコーダ２６によって各関節の現在の関節角を取得する（ステップＳ１００）。次にモータドライバ３８からアクチュエータ２２へ送られる電流指令値をモニタすることによって、各関節に作用する負荷の大きさと方向を推定する（ステップＳ１０２）。次に関節ごとに推定した負荷の大きさを閾値と比較する（ステップＳ１０４）。ここで、推定した負荷の大きさが閾値よりも大きい関節が過負荷関節として特定される。過負荷関節については、（１）式、と（２）式により目標関節角が決定される（ステップＳ１０６）。一方、ステップＳ１０４で過負荷関節が特定されるので、過負荷関節以外の関節が非過負荷関節として特定される（ステップＳ１０８）。非過負荷関節については、目標値ベクトルから、優先順位の高い順に非過負荷関節の数と同数の要素が抽出される（ステップＳ１１０）。抽出された要素を入力とし、非過負荷関節の目標関節角を出力とする逆変換式が選定される（ステップＳ１１２）。逆変換式には、ステップＳ１００で取得した現在関節角の値が代入される。そうして、逆変換式の具体的な数値が設定される。設定された逆変換式に抽出された要素の値を入力して非過負荷関節の目標関節角を決定する（ステップＳ１１４）。ステップＳ１０６で決定された過負荷関節の目標関節角とステップＳ１１４で決定された非過負荷関節の目標関節角がともにモータドライバ３８へ出力される（ステップＳ１１６）。図３には図示していないが、モータドライバ３８によって、各関節は現在関節角が目標関節角に追従するように制御される。以上の処理が制御サンプリング毎に繰り返される。

本実施例の多関節ロボット１００は６個の関節を有している。即ち、多関節ロボット１００は６自由度を有している。従って先端リンクの現在値ベクトルを６次元の目標値ベクトルに追従するように制御することができる。６個の関節全てが非過負荷関節の場合には、6個の関節すべてを６次元の目標値ベクトルへの追従制御に使用できる。即ち、手先を任意の目標値ベクトルに追従させることができる。ここで、ある関節の推定負荷の大きさが閾値を超える場合には、上記（２）式によりその関節は目標値ベクトルに関わりなく負荷の作用する方向に駆動される。即ち、過負荷関節は負荷が低減されるであろう方向に駆動される。
過負荷関節は目標値ベクトルに関わり無く負荷に応じて制御されるため、過負荷関節は先端リンクの現在値ベクトルを目標値ベクトルに追従させる制御に利用できなくなる。目標値ベクトルへの追従制御に利用できる関節（即ち非過負荷関節）の数は、目標値ベクトルの次数より少なくなる。従ってそのままでは先端リンクの現在値ベクトルを任意の目標値ベクトルに追従させることはできない。本実施例の多関節ロボットでは、目標値ベクトルの各要素に対して優先順位が予め設定されている。目標値ベクトルから優先順位の高い順に非過負荷関節と同数の要素が抽出される。即ち、追従制御に利用できる関節の数と同数まで目標値ベクトルを低次元化する。そうすることによって、先端リンクの現在値ベクトルを低次元化された目標値ベクトルに追従させる目標関節角が決定できる。目標値ベクトルを低次元化する際には、予め決められた優先順位によって、多関節ロボットに実行させる作業に応じてより正確に追従させたい要素が優先的に抽出される。本実施例の多関節ロボットでは、ある関節が過負荷状態となって先端リンクの現在値ベクトルを目標値ベクトルに完全に追従させることが困難となった場合でも、目標値ベクトルのうち重要な要素については高い追従性が維持されるように非過負荷関節を制御することができる。これにより、ある関節が過負荷状態となってもできるだけ作業を継続することのできる多関節ロボットが実現できる。本実施例の多関節ロボットは、どの関節が過負荷となっても、目標値ベクトルの要素のうち、常に高順位の要素について追従性が確保される。本実施例の多関節ロボットによれば、どの関節が過負荷関節となっても、予定されていた作業を可能な限り継続できる。
なお、本実施例では、目標関節角が目標駆動量の一態様に相当する。図２の負荷推定部３２が請求項の負荷推定部の一態様に相当する。過負荷関節特定部３４が請求項の過負荷関節特定部の一態様に相当する。力制御演算部３６が請求項の第１駆動量算出部の一態様に相当する。目標値要素抽出部４２と逆変換部４４が請求項の第２駆動量算出部の一態様に相当する。
本実施例では、負荷推定部３２、過負荷関節特定部３４、力制御演算部３６、目標値要素抽出部４２、及び逆変換部４４はコントローラ３０のＣＰＵが実行するコンピュータプログラムモジュールとして実装される。負荷推定部３２等は、夫々専用の処理を行う専用ハードウエアで実装されてもよい。

＜第２実施例＞ 次に第２実施例について説明する。図４に第２実施例の多関節ロボットのブロック図を示す。図４では、図２に示す第１実施例のブロック図の構成要素と同じ構成要素については同じ符号を付してある。図２と同じ符号を付したブロックについては説明を省略する。
第２実施例の多関節ロボットにおけるロボット本体１０ｂは、先端リンク（図１の第７リンク１２ｇ）に力センサ２８を有する。それ以外は第１実施例のロボット本体１０と同じであるので説明を省略する。力センサ２８は、手先効果器１４（図１参照）とロボット本体１０ｂの先端リンクの間に取り付けられている。即ち、力センサ２８は手先効果器１４を介してロボット本体１０ｂに加わる外力を検出する。

第２実施例のコントローラ３０ｂと第１実施例のコントローラ３０（図２参照）の相違点は負荷推定部３２である。負荷推定部３２ｂ以外は第１実施例のコントローラ３０と同じであるので説明を省略する。
本実施例の多関節ロボットでは、第１実施例と同様に各関節の駆動軸に作用する総負荷をモータドラバ３８からアクチュエータ２２へ指令される電流指令値から推定する。負荷推定部３２ｂには、力センサ２８の検出値とエンコーダ２６の検出値（各関節の現在の関節角）が入力される。負荷推定部３２ｂは、力センサによって検出される外力（即ち手先効果器１４を介してロボット本体１０ｂに加わる外力）を各関節の駆動軸への負荷トルク（手先外力による負荷トルク）に変換する。負荷推定部３２ｂは推定した総負荷の値から手先外力による負荷トルクの値を減じた値を最終的な推定負荷として出力する。このときの出力には推定負荷の大きさと方向が含まれる。なお手先効果器１４を介してロボット本体に加わる外力の各駆動軸への変換は、現在の関節角にから算出されるヤコビ行列の転置行列を用いて得られる。即ち、本実施例における負荷推定部３２ｂは、関節の駆動軸に作用する総負荷から力センサが検出する外力（手先効果器を介してロボット本体１０に作用する外力）による負荷を除いた負荷の大きさと方向を推定する。以下では、関節の駆動軸に作用する総負荷から力センサが検出する外力による負荷を除いた負荷を想定外負荷と称する。想定外負荷の原因となる外力を想定外外力と称する。想定外外力は、例えば多関節ロボットが作業中にその姿勢を変えることによって、ロボット本体の手先効果器以外の箇所が他の物体と接触した場合などに生ずる外力である。即ち、想定外外力に起因する想定外負荷の大きさが閾値を越えた場合には、ロボット本体が他の物体と予期せぬ接触を生じている可能性が高い。そのような場合にはロボットは他の物体との接触を回避するように動作させることが好ましい。本実施例のコントローラ３０ｂは、負荷推定部３２ｂを除いて第１実施例のコントローラ３０と同様である。従って負荷推定部３２ｂによって推定された負荷（想定外負荷）の大きさが閾値よりも大きい過負荷関節については、負荷の作用する方向と同じ方向に関節が駆動される。これによって、ロボットと他の物体との接触を解消する方向に関節を動作させることができる。なお、本実施例における閾値の値は、第１実施例における閾値の値と異なってよい。具体的には、本実施例では負荷推定部３２ｂは想定外外力に起因する想定外負荷を推定するので、閾値は想定外負荷を許容できる値に設定される。
本実施例の多関節ロボットは、手先効果器以外の部位で他の物体と接触した場合に、その接触を解消しながら、接触の解消に関与しない関節（即ち非過負荷関節）は、できるだけ先端リンクの現在値ベクトルを目標値ベクトルに追従させるように制御される。即ち、本実施例の多関節ロボットは、手先効果器以外の部位で他の物体と接触した場合に、その接触を解消しながら、ロボットが実行中の作業を可能な限り継続することができる。

＜第３実施例＞ 次に本発明の第３実施例について説明する。第１実施例と第２実施例では、非過負荷関節の目標関節角を決定する際に逆変換を用いた。第３実施例では、各関節を制御する際の目標値（目標駆動量）は関節角速度であり、目標値ベクトルは先端リンクの目標速度（図１の座標系におけるｘ、ｙ、ｚ各軸方向の速度及び、ロール角速度、ピッチ角速度、ヨー角速度）で表される。即ち、過負荷関節が存在しない場合には第３実施例の多関節ロボットはいわゆる分解速度制御が行なわれる。本実施例は、非過負荷関節の目標値（角速度）を決定する際に分解速度制御を応用する。なお、本実施例では、多関節ロボットのブロック図は図２を参照する。但し図２における逆変換部４４は、分解速度制御演算部に置き換える。

先端リンクの目標値ベクトルから目標関節角速度ベクトル（関節の目標駆動量ベクトル）へはヤコビ行列を用いて変換できる。目標値ベクトルをｄＸで表し、各関節の現在関節角を要素とする関節角ベクトルをΘで表し、各関節の目標角速度を要素とする目標関節角速度ベクトル（目標駆動量ベクトル）をｄΘで表し、ヤコビ行列をＪ（Θ）で表すと、それらの関係は次の（３）式で表せる。
ｄＸ＝Ｊ（Θ）・ｄΘ ・・・（３）
従って、目標関節角速度ベクトルｄΘは次の（４）式で表せる。
ｄΘ＝Ｊ^−１（Θ）・ｄＸ ・・・（４）
ここでＪ^−１（Θ）はヤコビ行列Ｊ（Θ）の逆行列である。第１実施例と同じく、多関節ロボットは６個の関節を有しており、目標値ベクトルは先端リンクのｘ軸、ｙ軸、ｚ軸方向の速度と、３軸夫々の軸周りのロール角速度、ピッチ角速度、及びヨー角速度を要素とする６次元ベクトルである。（３）式において、ｄＸとｄΘが共に６次元であるのでヤコビ行列Ｊ（Θ）は６×６の正方行列となる。従って、ヤコビ行列Ｊ（Θ）の行列式がゼロでない限り逆行列Ｊ^−１（Θ）が存在して（４）式によって目標値ベクトルｄＸからｄΘを求めることができる。各要素が（角）速度の単位を有する目標値ベクトルに対して、ヤコビ行列の逆行列を用いて目標関節角速度を算出し、得られた目標関節角速度を実現するように各関節のアクチュエータを制御する方法が分解速度制御法と呼ばれる制御法である。

本実施例において、過負荷関節が存在する場合の処理を説明する。今、図２の過負荷特定部３４によって、６次元の関節角ベクトルΘの第ｉ要素の関節が過負荷関節と特定されたとする。過負荷関節がひとつ存在するので、目標値要素抽出部４２は６次元の目標値ベクトルｄＸの要素のうち優先順位の上位から５個の要素を抽出する。換言すれば、５次元に低次元化した目標値ベクトルを抽出する。ここで、６次元の目標値ベクトルｄＸの要素のうち、第ｊ要素が最も優先順位の低い要素とする。本実施例の分解速度制御演算部は、第ｊ要素の欠けた５次元の目標値ベクトル（ｄＸ’と表す）から第ｉ要素の欠けた５次元の目標関節角速度ベクトル（ｄΘ’と表す）を決定する。（３）式に示した６×６のヤコビ行列Ｊ（Θ）から第ｊ行と第ｉ列を除外した５×５の低次元ヤコビ行列をＪ’（Θ）と表すと、次の（５）式が成立することがわかる。
ｄＸ’＝Ｊ’（Θ）・ｄΘ’ ・・・（５）
従って、５次元の目標関節角速度ベクトルｄΘ’は次の（６）式で求められる。
ｄΘ’＝Ｊ’^−１（Θ）・ｄＸ’ ・・・（６）
本実施例の分解速度制御演算部は、（６）式によって非過負荷関節（第ｉ番目の関節を除く関節）の目標関節角速度ベクトルｄΘ’を決定することができる。決定された目標関節角速度ベクトルｄΘ’の各要素（各非過負荷関節の目標関節角速度）はモータドライバ３８に送られる。モータドライバ３８は目標関節角速度に追従するようにアクチュエータ２２を制御する。なお、本実施例における力制御演算部３６は、駆動軸ごとにインピーダンス制御或いは力制御の制御則に基づいて、過負荷関節の目標関節角速度を算出する。
なお、上記例では、過負荷関節はひとつと仮定したが、過負荷関節が複数存在する場合も同様の議論が成立する。

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

以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。

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

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

第１実施例の多関節ロボットの概要図である。 第１実施例の多関節ロボットのブロック図である。 第１実施例の多関節ロボットの制御フローチャート図である。 第２実施例の多関節ロボットのブロック図である。

符号の説明

１０：ロボット本体
１２ａ、１２ｂ、１２ｃ、１２ｄ、１２ｅ、１２ｆ、１２ｇ：リンク
１４：手先効果器
２０ａ、２０ｂ、２０ｃ、２０ｄ、２０ｅ、２２ｆ：関節
２２ａ、２２ｂ、２２ｃ、２２ｄ、２２ｅ、２２ｆ：アクチュエータ
２６：エンコーダ
２８：力センサ
３０：コントローラ
３２：負荷推定部
３４：過負荷関節特定部
３６：力制御部
３８：モータドライバ
４０：記憶装置
４２：目標値要素抽出部
４４：逆変換部

Claims (5)

1. 多関節ロボット本体とコントローラを有する多関節ロボットであり、
   多関節ロボット本体は、
   複数のリンクと複数の関節と複数のアクチュエータを有しており、隣接するリンクが関節を介して連結されており、夫々のアクチュエータが夫々の関節を駆動することによって基部リンクに対する先端リンクの位置を変化させることが可能であり、
   コントローラは、
   先端リンクの目標値ベクトルと、目標値ベクトルの要素毎に設定されている優先順位を記憶している記憶部と、
   各関節の駆動軸に作用している負荷の大きさと方向を推定する負荷推定部と、
   推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾値以下の非過負荷関節を特定する過負荷関節特定部と、
   過負荷関節に対して、負荷の作用する方向と同じ方向に過負荷関節を駆動する目標駆動量を算出する第１駆動量算出部と、
   目標値ベクトルから優先順位の高い順に非過負荷関節と同数の目標値要素を抽出し、非過負荷関節に対して、抽出した目標値要素を実現するように非過負荷関節を駆動する目標駆動量を算出する第２駆動量算出部と、
   第１駆動量算出部と第２駆動量算出部が算出した目標駆動量に基づいてアクチュエータを制御するドライバを備えることを特徴とする多関節ロボット。
2. 各アクチュエータは通電している電流値と出力トルクが比例するモータであり、
   負荷推定手段は、ドライバがモータへ出力する電流指令値から負荷の大きさと方向を推定することを特徴とする請求項１の多関節ロボット。
3. 先端リンクに力センサが取り付けられており、負荷推定手段は、力センサが検出する外力から、各関節の駆動軸に作用している負荷の大きさと方向を推定することを特徴とする請求項１の多関節ロボット。
4. 先端リンクに力センサが取り付けられており、負荷推定手段は、関節の駆動軸に作用する総負荷から力センサが検出する外力による負荷を除いた負荷の大きさと方向を推定することを特徴とする請求項１に記載の多関節ロボット。
5. 複数のリンクと複数の関節と複数のアクチュエータを有しており、隣接するリンクが関節を介して連結されており、夫々のアクチュエータが夫々の関節を駆動することによって基部リンクに対する先端リンクの位置を変化させることが可能である多関節ロボットの制御コンピュータを、
   各関節の駆動軸に作用している負荷の大きさと方向を推定する負荷推定手段と、
   推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾値以下の非過負荷関節を特定する過負荷関節特定手段と、
   負荷の作用する方向と同じ方向に過負荷関節を駆動する過負荷関節の目標駆動量を算出する第１駆動量算出手段と、
   与えられた目標値ベクトルから、与えられた優先順位の高い順に非過負荷関節と同数の目標値要素を抽出し、抽出した目標値要素を実現するように非過負荷関節を駆動する非過負荷関節の目標駆動量を算出する第２駆動量算出手段と、
   第１駆動量算出手段と第２駆動量算出手段が算出した目標駆動量を、アクチュエータを制御するドライバへ出力する出力手段として機能させるための多関節ロボット用制御プログラム。

Images