トップ :: B 処理操作 運輸 :: B25 手工具;可搬型動力工具;手工具用の柄;作業場設備;マニプレ−タ

【発明の名称】 多関節ロボット及びその制御プログラム
【発明者】 【氏名】中島 裕一朗

【要約】 【課題】関節に作用する負荷が過負荷の場合に過負荷状態を緩和しながら作業をできるだけ継続し得るように関節を制御する多関節ロボットを提供する。

【構成】負荷推定部32によって各関節に作用する負荷を推定し、過負荷特定部34によって推定負荷が閾値より大きい過負荷関節を特定する。記憶装置40にはロボットの先端リンクの目標値ベクトルとそのベクトルの要素に対して予め設定された優先順位が記憶されている。目標値要素抽出部42は優先順位の高い順に非過負荷関節と同数の要素を目標値ベクトルから抽出する。力制御演算部36によって負荷と同じ方向に関節を駆動する目標駆動量が過負荷関節に対して決定される。逆変換部によって、抽出された目標値要素を実現する目標駆動量が非過負荷関節に対して決定される。過負荷関節については負荷を緩和する方向に制御しながら、非過負荷関節については目標ベクトルにできるだけ追従するように制御できる。
【特許請求の範囲】
【請求項1】
多関節ロボット本体とコントローラを有する多関節ロボットであり、
多関節ロボット本体は、
複数のリンクと複数の関節と複数のアクチュエータを有しており、隣接するリンクが関節を介して連結されており、夫々のアクチュエータが夫々の関節を駆動することによって基部リンクに対する先端リンクの位置を変化させることが可能であり、
コントローラは、
先端リンクの目標値ベクトルと、目標値ベクトルの要素毎に設定されている優先順位を記憶している記憶部と、
各関節の駆動軸に作用している負荷の大きさと方向を推定する負荷推定部と、
推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾値以下の非過負荷関節を特定する過負荷関節特定部と、
過負荷関節に対して、負荷の作用する方向と同じ方向に過負荷関節を駆動する目標駆動量を算出する第1駆動量算出部と、
目標値ベクトルから優先順位の高い順に非過負荷関節と同数の目標値要素を抽出し、非過負荷関節に対して、抽出した目標値要素を実現するように非過負荷関節を駆動する目標駆動量を算出する第2駆動量算出部と、
第1駆動量算出部と第2駆動量算出部が算出した目標駆動量に基づいてアクチュエータを制御するドライバを備えることを特徴とする多関節ロボット。
【請求項2】
各アクチュエータは通電している電流値と出力トルクが比例するモータであり、
負荷推定手段は、ドライバがモータへ出力する電流指令値から負荷の大きさと方向を推定することを特徴とする請求項1の多関節ロボット。
【請求項3】
先端リンクに力センサが取り付けられており、負荷推定手段は、力センサが検出する外力から、各関節の駆動軸に作用している負荷の大きさと方向を推定することを特徴とする請求項1の多関節ロボット。
【請求項4】
先端リンクに力センサが取り付けられており、負荷推定手段は、関節の駆動軸に作用する総負荷から力センサが検出する外力による負荷を除いた負荷の大きさと方向を推定することを特徴とする請求項1に記載の多関節ロボット。
【請求項5】
複数のリンクと複数の関節と複数のアクチュエータを有しており、隣接するリンクが関節を介して連結されており、夫々のアクチュエータが夫々の関節を駆動することによって基部リンクに対する先端リンクの位置を変化させることが可能である多関節ロボットの制御コンピュータを、
各関節の駆動軸に作用している負荷の大きさと方向を推定する負荷推定手段と、
推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾値以下の非過負荷関節を特定する過負荷関節特定手段と、
負荷の作用する方向と同じ方向に過負荷関節を駆動する過負荷関節の目標駆動量を算出する第1駆動量算出手段と、
与えられた目標値ベクトルから、与えられた優先順位の高い順に非過負荷関節と同数の目標値要素を抽出し、抽出した目標値要素を実現するように非過負荷関節を駆動する非過負荷関節の目標駆動量を算出する第2駆動量算出手段と、
第1駆動量算出手段と第2駆動量算出手段が算出した目標駆動量を、アクチュエータを制御するドライバへ出力する出力手段として機能させるための多関節ロボット用制御プログラム。
【発明の詳細な説明】【技術分野】
【0001】
本発明は、多関節ロボットとその制御プログラムに関する。多関節ロボットはロボット本体と、その本体を制御するためのコントローラからなる。本明細書では、ロボット本体を表す場合にも多関節ロボットと称することがある。或いは単にロボットと称することがある。本明細書にいう多関節ロボットは、複数のリンクと複数の関節を有しており、隣接するリンクが関節を介して連結されており、基部リンクと先端リンクの間に少なくとも2以上の関節が介在しているロボットをいう。ここでいう先端リンクとは目標値ベクトルが与えられるリンクを意味し、必ずしも物理的に先端に配置されるリンクに限らない。例えば、5指を含む腕を模擬した多関節ロボットであっても、掌リンクの目標値ベクトルが与えられる場合は、掌リンクの先に指リンク群が連結されていても掌リンクが先端リンクとなる。また本明細書では1自由度を1関節とカウントし、隣接するリンクを2軸周りに回転可能な関節は2つの関節から構成されているとする。
【背景技術】
【0002】
n個の関節を有する多関節ロボット(即ち、n自由度を有する多関節ロボット)は、各関節を独立に制御することによって、先端リンクの現在値ベクトルを、n以下の次元で記述される目標値ベクトルに一致にさせることができる。本明細書では「先端リンクの現在値ベクトルを目標値ベクトルに一致にさせる」との表現を単純化して「先端リンクを目標値ベクトルに一致させる」と表現する。目標値ベクトルは、位置(姿勢)の単位で与えられることもあれば、速度(角速度)の単位で与えられることもある。或いは加速度(加速度)の単位や力(トルク)の単位で与えられることもある。例えば、目標値ベクトルが6次元を有しており、位置(姿勢)の単位で与えられる場合には、目標値ベクトルの要素は直交座標系の各座標軸方向の位置と、所謂ロール角、ピッチ角、及びヨー角の6個のスカラー値で与えられる。目標値ベクトルの要素は様々な単位系を取り得るため、本明細書では目標位置ベクトルや目標速度ベクトルなどという表現の代わりに単位系を特定しない目標値ベクトルという表現を用いる。先端リンクの現在値ベクトルは、先端リンクの物理的状態を、目標値ベクトルと同じ単位系で記述したベクトルとなる。
なお、目標値ベクトルの次数をn以下と表現したのは、本明細書で扱う多関節ロボットは、いわゆる冗長自由度を有する多関節ロボットであってもよいからである。
【0003】
n自由度の多関節ロボットの先端リンクに対してn以下の目標値ベクトルが与えられると、先端リンクを目標値ベクトルに一致させるための各関節の目標駆動量を決定することができる。関節の目標駆動量の単位は、位置(角度)や速度(角速度)など、様々な単位系を取り得る。本明細書における「目標駆動量」との表現は、関節の目標関節角や目標角速度といった、関節の「目標値」を含む意味で用いる。従って、例えば関節の現在関節角を目標関節角に一致するように関節を駆動することを、「関節を目標駆動量に一致させる」と表現する場合がある。
先端リンクを目標値ベクトルに一致させるための各関節の目標駆動量は、いわゆる逆変換やヤコビ行列によって求められることは良く知られている。関節の目標駆動量が求まると、関節の現在値(現在関節角や現在関節角速度など)が関節の目標駆動量(目標関節角や目標関節角速度など)に一致するように、関節を駆動するためのアクチュエータが制御される。ここに「関節を駆動する」とは、関節に連結されたリンク同士の相対位置(姿勢)を変化させることである。回転関節の場合は関節に連結されたリンク間の角度(関節角)を変化させることに相当する。直動関節の場合は関節に連結されたリンクの間の距離を変化させることに相当する。
【0004】
ロボットの先端リンクを目標値ベクトルに一致させることによって、ロボットに所定の作業を行わせることができる。例えばグリッパや溶接ツールなどの手先効果器をロボットの先端リンクに固定し、先端リンクを経時的に変化する目標値ベクトルに一致(追従)させることによって、その手先効果器を所望の軌道に沿って移動させて所定の作業を行わせることができる。或いは、多関節ロボットが脚式ロボットである場合には、先端リンクに相当する足平リンクを目標値ベクトルに追従させることによって、歩行という作業を実行させることができる。
【0005】
ところで、ロボットの作業中に、関節に作用する負荷が増大する可能性がある。例えば、ロボットが予期せぬ物体に接触したにもかかわらず、時間的に変化する目標値ベクトルに追従する制御を継続すると、結果的にロボットが物体を押しのけることになる。ロボットが物体を押しのける際に関節に作用する負荷が増大する恐れがある。あるいは、ロボットが先端リンクに取り付けられたグリッパによって物体を持ち上げる作業を行う場合に、持ち上げる物体の質量が予想以上に大きいと、関節に作用する負荷も予想以上に増加する。関節に作用する負荷のうち、関節の駆動軸に作用している負荷成分については、その関節を駆動するアクチュエータにトルクリミッタなどの過負荷対策を設けることによって緩和することができる。以下では関節の駆動軸に作用する負荷を対象とする。また、本明細書にいう過負荷とは、関節に作用する負荷が予め設定された閾値を超える場合を意味する。即ち、本明細書にいう過負荷とは、関節の物理的な許容限度を超える負荷が関節に作用する場合に限られない。
過負荷対策として関節に装備されるトルクリミッタにはメカニカルなトルクリミッタとソフトウエア的なトルクリミッタがある。特許文献1には、ソフトウエア的なトルクリミッタの一例が開示されている。特許文献1に開示された技術は、アクチュエータであるモータを流れる電流を監視し、通電電流が所定値を超えた場合にモータに通電する電流の供給を停止するという技術である。
【0006】
【特許文献1】特開2005−161469号公報
【発明の開示】
【発明が解決しようとする課題】
【0007】
ある関節が過負荷となりトルクリミッタが作動すると、その関節は目標駆動量に基づいて制御することができなくなる。
前述したように、多関節ロボットは、先端リンクを目標値ベクトルに一致させるように各関節の目標駆動量が決定される。目標値ベクトルの夫々の要素の値は、通常、複数の関節の目標駆動量の関数となる。しかも、ロボットの全ての関節が先端リンクを目標リンクに一致させるために利用できるという前提で個々の関節の目標駆動量が決定される。従って、ひとつでも関節が過負荷となり、その関節を目標駆動量に一致させることができなくなると、目標値ベクトルの複数の要素について、先端リンクの現在値ベクトルの要素を対応する目標値ベクトルの要素に一致させることができなくなる。即ち、ロボットの作業が中断してしまう。
【0008】
ひとつの関節が過負荷になっても他の関節が過負荷となっているとは限らない。むしろ過負荷になっていない関節の数が、過負荷になった関節よりも多い可能性が高い。過負荷になっていない関節が残っているならば、それらの関節を適切に駆動することによって、ロボットの先端リンクを目標値ベクトルに追従させる制御をできるだけ継続し、ロボットに予定していた作業を続行させることが好ましい。
従来の技術では、ひとつの関節でも過負荷となれば、過負荷状態を解消することのみに着目しており、過負荷状態を解消しながら、予定していた作業をできるだけ継続させるものでない。関節が過負荷となった場合に、その過負荷状態を解消しながら、過負荷となっていない関節を活用し、予定されたロボット作業を継続し得る多関節ロボットが望まれている。
【課題を解決するための手段】
【0009】
ロボットの先端リンクの現在値ベクトルを任意の目標値ベクトルに追従させるには、目標値ベクトルの要素の数に対して任意の関節角に制御可能な関節の数がそれ以上であることが要求される。即ち、n次元の目標値ベクトルに対してはロボットにはn以上の自由度が要求される。そこで通常は目標値ベクトルの次元数に応じてロボットの自由度数が決定される。例えば目標値ベクトルがn次元であればn自由度を有する多関節ロボットが用意される。ロボットの作業中にある関節が過負荷となった場合、その関節はもはや先端リンクを目標値ベクトルに一致させるための制御には利用できないので、先端リンクを目標値ベクトルに一致させる制御に利用できる関節数がn個を下回ることになる。その結果、ロボットの先端リンクを目標値ベクトルへ追従させることができなくなる。
【0010】
発明者は、ロボットに行なわせる作業の種類に応じて、先端リンクの目標値ベクトルの要素には、現在値ベクトルのうち対応する現在値要素をできるだけ正確に一致させたい要素と、それほど正確に一致させなくともよい要素があることに着目した。例えば、先端リンクに取り付けられた手先効果器で水の入ったコップを把持して移動させる作業を考察する。このとき、目標値ベクトルは先端リンクの空間6次元(3軸の直交座標系における各軸方向の目標位置と各軸周りの目標姿勢角)の6要素を有すると仮定する。先端リンクの目標姿勢角はコップ内の水がこぼれないように設定されている。この場合、先端リンクの水平面内の2軸周りの姿勢角(即ち、コップの傾き角)のいずれか一方でも目標姿勢角から外れるとコップ内の水がこぼれてしまう可能性がある。一方、先端リンクの鉛直方向の軸周りの姿勢角(即ち、コップの鉛直軸周りの回転角)は目標姿勢角から外れても水がこぼれることは無い。さらに鉛直軸周りの姿勢角が変化しても、コップの3軸方向の位置は変化しない。水をこぼさずにコップを移動させる作業において、先端リンクの鉛直軸周りの姿勢角が目標姿勢角に一致しなくとも、現在値ベクトルの他の要素(即ち、先端リンクの3軸方向の位置と水平面内の2軸周りの姿勢角の5要素)が目標値ベクトルの対応する要素に一致すれば、コップの移動作業は完遂することができる。即ち、ある関節のアクチュエータが過負荷となってその関節を先端リンクを目標値ベクトルに一致させる制御に利用できなくなった場合には、先端リンクの鉛直軸周りの現在姿勢角の目標姿勢角への追従性を犠牲にしても現在値ベクトルの他の要素について、目標値ベクトルの対応する要素への追従性が維持されるように残りの関節を制御すれば作業を継続することが可能となる。
そこで本発明では、ロボットの先端リンクの目標値ベクトルの各要素に対して優先順位を設定しておく。目標値ベクトルの要素のうち、その要素に対応する現在値をできるだけ正確に追従させたい要素には高い優先順位を設定し、高い追従性が要求されない要素には低い優先順位を設定する。優先順位はロボットに行なわせる作業の種類に応じて事前に設定すればよい。そうしておいて、ロボットの作業中に次の処理を行う。過負荷状態にある関節を特定し、その関節については目標ベクトルに関わらずに過負荷状態を解消するように関節の目標駆動量を決定する。例えば、関節の目標駆動量が目標関節角として設定されるならば、作用している負荷の方向と同じ方向に関節を回転させるように目標関節角を決定すればよい。
一方、過負荷となっていない非過負荷関節の数を特定し、目標値ベクトルから目標値要素を優先順位の高い順に非過負荷関節と同数だけ抽出する。抽出した目標値要素の数と非過負荷関節の数は同数であるため、非過負荷関節については、抽出した目標値要素を実現するための目標駆動量を決定することができる。
【0011】
本発明は、多関節ロボットの自由度数は目標値ベクトルの次数以上でなければならないとのこれまでの通念に対して、先端リンクを目標値ベクトルに一致させる制御に利用できる関節の数と同数まで目標値ベクトルを低次元化するという新たな発想に基づく。さらに目標値ベクトルの要素には追従性に関して優先順位があることに着目し、目標値ベクトルの要素に優先順位を予め設定する。設定された優先順位の低い順に切り捨てて目標値ベクトルを低次元化することによって、作業にとって重要な目標値要素の追従性を維持する。一方、過負荷関節については、目標値ベクトルに関わらず、過負荷状態を緩和するように関節の目標駆動量を設定する。
過負荷関節の過負荷状態を緩和しながら当初に予定されていた作業をできるだけ継続し得る多関節ロボットを実現することに成功した。
なお、上記の議論は目標値ベクトルが与えられたとき、その単位系に依存しない。即ち、目標値ベクトルの要素は位置や角度(姿勢角)だけでなくそれらの速度や加速度であってもよい。このことは関節の目標駆動量が角度の単位に限られず角速度や角加速度やトルクの単位であっても成立することを意味する。或いは、関節が直動関節の場合には、関節の目標駆動量が伸展長さ、伸展速さ、伸展加速度、伸展させる力であっても成立する。
【0012】
本発明は、多関節ロボット本体とコントローラを有する多関節ロボットに具現化できる。多関節ロボット本体は、複数のリンクと複数の関節と複数のアクチュエータを有しており、隣接するリンクが関節を介して連結されており、夫々のアクチュエータが夫々の関節を駆動することによって基部リンクに対する先端リンクの位置を変化させることが可能に構成されている。
コントローラは、記憶部、負荷推定部、過負荷関節特定部、第1駆動量算出部、第2駆動量算出部、及びドライバを備える。
記憶部には先端リンクの目標値ベクトルと、目標値ベクトルの要素毎に設定されている優先順位が記憶されている。負荷推定部は、各関節の駆動軸に作用している負荷の大きさと方向を推定する。過負荷関節特定部は、推定した負荷の大きさが閾値より大きい過負荷関節と、推定した負荷の大きさが閾値以下の非過負荷関節を特定する。第1駆動量算出部は、過負荷関節に対して、負荷の作用する方向と同じ方向に過負荷関節を駆動する目標駆動量を算出する。一方、第2駆動量算出部は、目標値ベクトルから優先順位の高い順に非過負荷関節と同数の目標値要素を抽出し、非過負荷関節に対して、抽出した目標値要素を実現するように非過負荷関節を駆動する目標駆動量を算出する。そしてドライバは、第1駆動量算出部と第2駆動量算出部が算出した目標駆動量に基づいてアクチュエータを制御する。
各関節の駆動軸に加わる負荷とは、回転関節の場合は駆動軸に加わるトルクであり、直動関節の場合は駆動軸の伸びる方向に加わる力である。
負荷推定部は、回転関節の場合には関節軸に取り付けられたトルクセンサからの信号を受信し、受信した信号に所定の係数を乗じて負荷の大きさと方向を推定する演算部でよい。或いは、アクチュエータが電流駆動のモータの場合には、ドライバがモータへ出力する電流指令値からアクチュエータの出力トルクの大きさと方向を推定し、その出力トルクから駆動軸に加わる負荷の大きさと方向を推定する推定器でもよい。電流駆動のモータは、通電する電流値と出力トルクがほぼ比例関係を有する場合が多い。そのようなモータの場合は、ドライバがモータへ出力する電流値にいわゆるトルク定数を乗じることで、モータの出力トルクを推定できる。モータの出力トルクに、モータ軸から関節駆動軸へのトルク伝達定数を乗じることによって、関節駆動軸の出力トルクを推定できる。関節駆動軸の出力トルクの大きさを負荷の大きさの推定値としてよい。出力トルクの方向に対して逆方向を負荷の方向の推定値としてよい。或いは、先端リンクに力センサを取り付け、力センサが検出する外力の大きさと方向から各関節の駆動軸に作用する負荷の大きさと方向を推定してもよい。力センサが検出する外力から各関節の駆動軸に作用する負荷への変換は、良く知られているようにヤコビ行列を用いて算出できる。
関節の目標駆動量は前述したように、回転関節の場合は関節角、関節角速度、関節角加速度、あるいはトルクのいずれかであってよい。直動関節の場合は、伸展長さ、伸展速さ、伸展加速度、或いは伸展させる力のいずれかであってよい。また、目標値ベクトルは経時的に変化するものであってもよい。また、閾値は関節ごとに異なる値でよいし、経時的に変化するものであってもよい。
また、推定した負荷の大きさが閾値よりも大きい関節を過負荷関節と特定し、推定した負荷の大きさが閾値以下である関節を非過負荷関節と特定する。ここで、「閾値よりも大きい」と「閾値以下である」との分類を、「閾値以上である」と「閾値未満である」との分類に変更しても本発明の技術的範囲を超えるものではないことを明記する。本発明では、閾値を設けることは重要であるが、閾値と同じ負荷である関節を非過負荷関節に分類するのは単に数学的に矛盾なく閾値の上下で関節を分類するための便宜にすぎない。閾値と同じ負荷である関節を過負荷関節に分類しても本発明の特徴はなんら変更されるものではない。
【0013】
上記構成では、過負荷関節特定部が、全ての関節を閾値を越える負荷が加わっている過負荷関節とそうでない非過負荷関節に分ける。過負荷関節については、第1駆動量算出部によって、目標ベクトルに関わらずに負荷と同じ方向に過負荷関節を駆動する目標駆動量が決定される。過負荷関節を負荷が作用する方向と同じ方向に駆動すれば、負荷を低減できる可能性が高いからである。一方、過負荷となっておらず目標値ベクトルへの追従制御に利用できる非過負荷関節については、第2駆動量算出部によって、可能な範囲で先端リンクを目標値ベクトルに追従させるように非過負荷関節を駆動する目標駆動量を決定する。具体的には、非過負荷関節の数と同数の要素を目標値ベクトルから抽出する。非過負荷関節の数と抽出した要素数が一致するので、抽出した要素の目標値を実現することのできる目標駆動量が決定できる。さらに、目標値ベクトルの要素は、予め設定された優先順位の高い順に抽出される。即ち、どの関節が過負荷となっても、追従性が重要視される要素が常に抽出され、その要素については現在値の目標値への追従性が確保される。即ち、予定されていた作業の動作に近い動作が実現できる。
各関節の目標駆動量の決定は、目標駆動量が関節角で表される場合には逆変換(逆運動学)により決定することができる。或いは順変換の繰り返し計算によっても決定することができる。また目標駆動量が関節角速度で表される場合にはいわゆる分解速度制御法に基づいて決定することができる。
【0014】
ロボットに作業を行わせる際には、ロボットの先端リンクに手先効果器が取り付けられている場合が多い。手先効果器はグリッパ(多指ハンドを含む)、切削ツール、溶接ツールなど、作業目的に応じて選択される。そのような場合、手先効果器は作業対象物(例えば前述したコップ移動作業におけるコップなど)と接触するために、手先効果器を介してロボットに外力が加わる。そのような外力は予定されている外力である。その一方で、多関節ロボットは、各関節を動作させるとロボット全体の姿勢が変化する。その結果、手先効果器以外の箇所でロボットが他の物体と予想外に接触する可能性がある。接触によって発生する外力は低減できる方が好ましい。そこでロボットの先端リンクに力センサを取り付け、負荷推定手段は、関節の駆動軸に作用する総負荷から力センサが検出する外力による負荷を除いた負荷の大きさと方向を推定することが好ましい。力センサは例えば、先端リンクと手先効果器の間に取り付けられる。これによって、手先効果器を介してロボットに加わる外力を力センサによって検出することができる。力センサが検出する外力によって各関節軸に作用する負荷は前述したようにヤコビ行列により推定することができる。一方、関節の駆動軸に作用する総負荷は、前述したようにアクチュエータであるモータへの電流指令値にトルク定数を乗じることで推定できる。
関節の駆動軸に作用する総負荷から力センサが検出する外力による負荷を除いた負荷の大きさが閾値を越える過負荷関節については、第1駆動量算出部によって、負荷の作用する方向と同じ方向へ過負荷関節を駆動する目標駆動量が算出される。その目標駆動量に基づいて過負荷関節が駆動されることによって、ロボットは接触状態を解消する方向に姿勢を変化させることができる。同時に、第2駆動量算出部によって、非過負荷関節の目標駆動量として、できるだけ先端リンクを目標値ベクトルに一致させるための駆動量が算出される。そのように算出された目標駆動量に基づいて非過負荷関節が制御されることで、目標値ベクトルの要素のうち、優先順位が高い要素については追従性が確保される。以上により、ロボットがその先端以外で物体と接触したとき、その接触状態を解消しながら、先端リンクをできるだけ目標値ベクトルに一致させるように動作する多関節ロボットを実現することができる。
【発明の効果】
【0015】
本発明によれば、ある関節が過負荷となった場合に、その過負荷状態を解消しながら、ロボットの先端リンクを目標値ベクトルにできるだけ追従する多関節ロボットを実現できる。この多関節ロボットによれば、ある関節が過負荷となった場合に、先端リンクの目標値ベクトルに対する追従性は、目標値ベクトルの要素のうち予め決められた優先順位の高い順から確保される。従って、どの関節が過負荷となっても、多関節ロボットが行う作業において重要な目標値ベクトルの要素については先端リンクの追従性(先端リンクの現在値要素の目標値ベクトル要素への追従性)が確保できる。その結果、多関節ロボットは、どの関節が過負荷となっても可能な限り作業を継続することができる。
【実施例】
【0016】
<第1実施例> 本発明の第1実施例に係る多関節ロボットについて、図面を参照して説明する。図1は、多関節ロボット100の概要図を示す。多関節ロボット100はロボット本体10とコントローラ30からなる。図1では、ロボット本体10を関節とリンクの連結構造を示すスケルトン図で表している。本実施例の多関節ロボット100は、初期位置にある水の入ったコップ(不図示)をグリッパ14で把持し、目的の位置へ目的の姿勢で位置決めするコップ移動作業を行うロボットである。本実施例の多関節ロボット100は、コップ移動作業中に夫々の関節に加わる負荷トルクの大きさが予め設定された閾値を超えた場合に、その関節については負荷を低減するように制御しながら、残りの関節を用いてコップ移動作業を可能な限り継続することができる。関節に加わる負荷トルクの大きさが閾値を超える場合とは、例えばロボット本体10のある部位が他の物体と接触してロボット10に想定外の外力が加わる場合に起こり得る。
【0017】
ロボット本体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はグリッパであり、コップなどの物体を把持することができる。
【0018】
ロボット本体10は、第1関節20aから第6関節20fまでの6個の関節を有する6自由度ロボットである。従って、多関節ロボット100は、その先端リンクである第7リンク12g(即ち、手先効果器14)を3次元空間内で直交3軸方向の位置(図1に示す座標系でx座標値、y座標値、z座標値)と3軸周りの回転角(図1に示す座標系でロール角、ピッチ角、ヨー角)の計6自由度を任意に制御することができる。換言すれば、多関節ロボット100は、6自由度多関節ロボットである。
【0019】
関節20の夫々にはエンコーダ(不図示)が取り付けられている。エンコーダによって、各関節の関節角を検出することができる。
コントローラ30には、エンコーダ(不図示)の検出信号が入力される。またコントローラ30からは各モータ22へ制御信号(電流指令値)が出力される。コントローラがモータ20を制御することによって、各関節が駆動され、手先効果器14を目的の位置へ目的の姿勢で位置決めすることができる。即ち、手先効果器14が把持するコップを目的の位置へ目的の姿勢で位置決めすることができる。
【0020】
図2に多関節ロボット100のブロック図を示す。コントローラ30には、負荷推定部32、過負荷関節特定部34、力制御演算部36、モータドライバ38、記憶装置40、目標値要素抽出部42、及び逆変換部44が備えられている。記憶装置40を除く各部は、コントローラ30が備えるCPU(不図示)が実行するプログラムモジュールとしてコントローラ30に実装されている。ロボット本体10には、アクチュエータ22とエンコーダ26が備えられている。アクチュエータ22とエンコーダ26は、夫々の関節に備えられている。エンコーダ26は、各関節の関節角(現在関節角)を検出する。なお、図2では手先効果器14の図示を省略している。
【0021】
記憶装置40には、第7リンク12g(先端リンク)の目標値ベクトルと優先順位と負荷閾値のデータが記憶されている。本実施例における目標位置ベクトルは、第7リンク12gに固定された座標系の絶対座標系に対する相対位置と相対姿勢角を要素に持つベクトルである。第7リンク12gに固定された座標系の絶対座標系に対する相対位置を目標位置と称し、相対姿勢角を目標姿勢角と称する。具体的には図1に示す直交3軸のXYZ座標系において、各座標軸方向の目標位置(x座標値、y座標値、及びz座標値)と、各座標軸周りの目標姿勢角(ロール角、ピッチ角、及びヨー角)の計6個の要素を有するベクトルである。目標値ベクトルは、経時的に変化する時系列データとして記憶されている。なお、本明細書では、目標値ベクトルの要素を単に目標値と称することがある。また、エンコーダ26の検出値から算出される第7リンク12gに固定された座標系の絶対座標系に対するx座標値、y座標値、z座標値、ロール角、ピッチ角、及びヨー角を要素に持つベクトルを先端リンクの現在値ベクトルと称する。現在値ベクトルの個々の要素を単に現在値と称することがある。
前述したようにロボット本体10は6個の関節を有するので、コントローラ30が6個の関節の夫々の関節角を適宜制御することによって、先端リンクの現在値ベクトルの各要素を目標値ベクトルの対応する各要素に追従(一致)させることができる。即ち、水の入ったコップを目標値ベクトルに沿って移動させるコップ移動作業が実行できる。なお、現在値ベクトルは、エンコーダ26が検出する各関節角の値から、いわゆる順変換によって求めることができる。
【0022】
記憶装置40に記憶された優先順位は、目標値ベクトルの6個の要素に対して予め設定されている。ここでいう優先順位とは、コントローラ30が先端リンクの現在値ベクトルを目標値ベクトルに追従させる制御を行なうときに、現在値ベクトルの全ての要素を目標値ベクトルの対応する要素に追従させることができない場合に、優先的に追従させる要素を指定するものである。本実施例では、水の入ったコップを移動させる作業であるので、コップの水をこぼさないことが重要である。従って本実施例では、図1に示す座標系において、ロール角、ピッチ角、z座標値、x座標値、y座標値、ヨー角の順に優先順位が設定されている。ここでロール角とピッチ角の優先順位が高いのは、これらの角度が目標値からはずれるとコップが水平軸に対して傾くため、コップ内の水がこぼれる可能性があるからである。一方、ヨー角が最も低い優先順位に設定されているのは、Z軸(即ち鉛直方向に沿った軸)周りにコップが回転してもコップの水はこぼれることは無く、またヨー角が変化しても、コップ(先端リンク)のx座標値、y座標値、及びz座標値は変わらないからである。3軸方向の位置に関してz座標値がx座標値とy座標値よりも優先順位が高いのは、本実施例の作業が、コップを初期位置から持ち上げる−移動する−コップを下ろす、という作業であるため、コップの水平方向の位置がずれてもコップの垂直方向の位置が目標値からずれないようにするためである。上記の通り優先順位を設定することによって、コップ移動作業中に最も優先順位の低いヨー角の現在値が目標値に追従できなくなっても、その他の現在値が目標値に追従すれば、コップ移動作業を続行することができる。また、ヨー角の他に次の優先順位のy座標値の現在値が目標値に追従できなくなっても、ヨー角とy座標値以外の現在値が目標値に追従すれば、水をこぼすことなくコップを少なくともx座標値とz座標値については目標位置に移動させることができる。即ち、いくつかの関節が過負荷となって目標値ベクトルへの追従制御に利用できなくなっても、予定された作業を可能な限り継続することができる。
【0023】
ここで、現在値ベクトルの全ての要素を目標値ベクトルの対応する要素に追従させることができない場合とは、後述するように、関節に作用する負荷の大きさが閾値を超えた場合にその関節は負荷を低減するように制御されるため目標値ベクトルへの追従制御に利用できなくなる場合である。目標値ベクトルへの追従制御に利用できる関節の数が6未満となると、先端リンクの現在値ベクトルの6個の要素を夫々独立に制御することができなくなるからである。
【0024】
記憶装置40には、関節ごとに夫々の関節が許容できる負荷の大きさが閾値として記憶されている。閾値は、モータがオーバヒートしないための条件や、或いは、ロボットが他の物体に接触した場合にロボット又は他の物体にダメージを与えないための条件などから予め設定され記憶装置40に記憶される。
【0025】
次にコントローラ30が実行する制御について説明する。まず、通常の状態(即ち全ての関節において作用する負荷の大きさが閾値を下回っている状態)における制御について説明する。記憶装置40に記憶された目標値ベクトルが所定周期毎に逆変換部44へ入力される。なお、目標値要素抽出部42が行なう処理については後述する。逆変換部44にはまた、ロボット本体10の各関節に取り付けられたエンコーダ26が計測する関節角(現在関節角)が入力される。逆変換部44は、現在関節角を用いて、目標値ベクトルを各関節の目標関節角(目標関節角ベクトル)へ変換する逆変換を行う。逆変換式は、ロボット本体10のリンク構造が既知であり、各制御時における各関節の現在関節角が取得できれば求めることができる。本実施例では、多関節ロボットの関節の総数が6であり、目標値ベクトルの総要素数も6であるので、任意の目標値ベクトルを実現する目標関節角を求める逆変換式が設定される。逆変換部44によって求められた6個の目標関節角はモータドライバ38に送られる。モータドライバ38は、関節ごとに現在関節角が目標関節角に追従するようにアクチュエータ22を制御する。以上の制御は従来の多関節ロボットの制御と同じである。
【0026】
次に、関節に作用する負荷の大きさが閾値よりも大きい場合の制御について説明する。関節に作用する負荷の大きさと方向は負荷推定部32によって推定される。本実施例では、モータドライバ38の最下層の制御ループは電流制御ループである。アクチュエータであるモータの出力トルクは、モータに通電される電流値にほぼ比例する。従って、モータドライバ38がアクチュエータ22に送る電流指令値をモニタすることによって、アクチュエータの出力トルクの大きさと方向を推定することができる。アクチュエータの出力トルクと、関節軸が出力するトルクの関係は予め調べることができる。負荷推定部32は、モータドライバ38がアクチュエータへ出力する電流指令値をモニタすることによって、各関節のトルク、即ち、各関節に作用する負荷トルクの大きさと方向を推定する。
【0027】
負荷推定部32によって推定された各関節の推定負荷の大きさと方向は過負荷関節特定部34へ送られる。過負荷関節特定部34は、記憶装置40から各関節の閾値を読み出し、関節ごとに推定負荷の大きさと比較する。ここで、推定負荷の大きさが閾値より大きい場合にその関節を過負荷関節と称する。また、過負荷関節でない関節を非過負荷関節と称する。なお、ここでいう「過負荷関節」とは、推定負荷の大きさが予め設定された閾値を超えた関節を意味し、推定負荷の大きさが関節の物理的な許容負荷の大きさを超える関節に限らないことに留意されたい。過負荷関節特定部34によって、多関節ロボット本体10のすべての関節は、過負荷関節か非過負荷関節のどちらかに特定される。
過負荷関節については力制御演算部36によって、その目標関節角が決定される。非過負荷関節については逆変換部44によってその目標関節角が決定される。
【0028】
力制御演算部36では、過負荷関節について、負荷が作用する方向と同じ方向に関節を駆動する目標関節角が決定される。本実施例では、力制御ロジックは次の(1)式で表される。
τ=K・(θd−θc) ・・・(1)
ここで、τは負荷推定部32で推定された負荷である。Kはバネ定数であり、予め設定されている。θcはエンコーダ26によって検出された関節角(現在関節角)である。θdは目標関節角である。(1)式から目標関節角θdは次の(2)式で決定される。
θd=θc+τ/K ・・・(2)
(2)式の意味は次の通りである。目標関節角θdは、検出された現在の関節角θcに対して、負荷τが作用する方向にτ/Kだけ回転させる位置に決定される。換言すれば、目標関節角θdは、推定負荷τが関節駆動軸に作用する方向と同じ方向に関節を駆動するように決定される。ある関節の駆動軸に作用する負荷の大きさがロボット本体と他の物体との接触により増大した場合には、その関節を推定負荷τが作用する方向と同じ方向に回転させる目標関節角を設定し、現在関節角を目標関節角に追従するように制御することによって、関節に作用する負荷の大きさを低減することができる。推定負荷の大きさが閾値を超える関節(即ち、過負荷関節)の全てについて(1)式及び(2)式により目標関節角が決定される。(1)式と(2)式に表れる変数(τ、K、θd、及びθc)は関節ごとに異なる値であってよい。
【0029】
一方、過負荷関節特定部34の処理結果(即ち、どの関節が過負荷関節であるかの情報)は目標値要素抽出部42と逆変換部44に送られる。より具体的には、目標値要素抽出部42には、過負荷関節の数を示すデータが送られる。過負荷関節の数を示すデータは即ち非過負荷関節の数を示すデータと等価である。逆変換部44には、多関節ロボットが有する関節の中で過負荷関節を特定するデータが送られる。目標値要素抽出部42はまた、目標値ベクトルと優先順位のデータを記憶装置40から読み出す。目標値要素抽出部42では、目標値ベクトルの要素の中から、優先順位の高い順に非過負荷関節の数と同数の要素を抽出する。目標値要素抽出部42は抽出した目標値要素を逆変換部44へ送る。逆変換部44には過負荷特定部34から過負荷関節を特定するデータが送られるため、逆変換部44では、非過負荷関節を特定することができる。ここで、逆変換部44には予め複数の逆変換式(逆変換式群)が設定されている。その逆変換式群とは、目標値ベクトルの要素の任意の組み合わせを入力とし、目標関節角ベクトルの要素の任意の組み合わせを出力とする逆変換式の集合である。それらの逆変換式群は、多関節ロボットの幾何学的構造から予め求めておくことができる。逆変換部44では、それらの逆変換式群から、目標値要素抽出部42が抽出した要素を入力とし、非過負荷関節の目標関節角を出力とする一の逆変換式を選定する。選定された逆変換式を用いることによって、目標値要素抽出部42で抽出された要素を入力としたときの非過負荷関節の目標関節角を求めることができる。即ち、逆変換部44で非過負荷関節の目標関節角が決定される。
【0030】
以上の処理により、過負荷関節の目標関節角は力制御演算部36で決定され、非過負荷関節の目標関節角は逆変換部44で決定される。夫々決定された目標関節角はモータドライバ38に送られる。モータドライバ38では、前述した通常状態(過負荷関節がない場合)と同様に、夫々の関節について、現在関節角が決定された目標関節角に追従するようにアクチュエータ22が制御される。
【0031】
上記の処理の流れを図3のフローチャート図を参照して説明する。まず、エンコーダ26によって各関節の現在の関節角を取得する(ステップS100)。次にモータドライバ38からアクチュエータ22へ送られる電流指令値をモニタすることによって、各関節に作用する負荷の大きさと方向を推定する(ステップS102)。次に関節ごとに推定した負荷の大きさを閾値と比較する(ステップS104)。ここで、推定した負荷の大きさが閾値よりも大きい関節が過負荷関節として特定される。過負荷関節については、(1)式、と(2)式により目標関節角が決定される(ステップS106)。一方、ステップS104で過負荷関節が特定されるので、過負荷関節以外の関節が非過負荷関節として特定される(ステップS108)。非過負荷関節については、目標値ベクトルから、優先順位の高い順に非過負荷関節の数と同数の要素が抽出される(ステップS110)。抽出された要素を入力とし、非過負荷関節の目標関節角を出力とする逆変換式が選定される(ステップS112)。逆変換式には、ステップS100で取得した現在関節角の値が代入される。そうして、逆変換式の具体的な数値が設定される。設定された逆変換式に抽出された要素の値を入力して非過負荷関節の目標関節角を決定する(ステップS114)。ステップS106で決定された過負荷関節の目標関節角とステップS114で決定された非過負荷関節の目標関節角がともにモータドライバ38へ出力される(ステップS116)。図3には図示していないが、モータドライバ38によって、各関節は現在関節角が目標関節角に追従するように制御される。以上の処理が制御サンプリング毎に繰り返される。
【0032】
本実施例の多関節ロボット100は6個の関節を有している。即ち、多関節ロボット100は6自由度を有している。従って先端リンクの現在値ベクトルを6次元の目標値ベクトルに追従するように制御することができる。6個の関節全てが非過負荷関節の場合には、6個の関節すべてを6次元の目標値ベクトルへの追従制御に使用できる。即ち、手先を任意の目標値ベクトルに追従させることができる。ここで、ある関節の推定負荷の大きさが閾値を超える場合には、上記(2)式によりその関節は目標値ベクトルに関わりなく負荷の作用する方向に駆動される。即ち、過負荷関節は負荷が低減されるであろう方向に駆動される。
過負荷関節は目標値ベクトルに関わり無く負荷に応じて制御されるため、過負荷関節は先端リンクの現在値ベクトルを目標値ベクトルに追従させる制御に利用できなくなる。目標値ベクトルへの追従制御に利用できる関節(即ち非過負荷関節)の数は、目標値ベクトルの次数より少なくなる。従ってそのままでは先端リンクの現在値ベクトルを任意の目標値ベクトルに追従させることはできない。本実施例の多関節ロボットでは、目標値ベクトルの各要素に対して優先順位が予め設定されている。目標値ベクトルから優先順位の高い順に非過負荷関節と同数の要素が抽出される。即ち、追従制御に利用できる関節の数と同数まで目標値ベクトルを低次元化する。そうすることによって、先端リンクの現在値ベクトルを低次元化された目標値ベクトルに追従させる目標関節角が決定できる。目標値ベクトルを低次元化する際には、予め決められた優先順位によって、多関節ロボットに実行させる作業に応じてより正確に追従させたい要素が優先的に抽出される。本実施例の多関節ロボットでは、ある関節が過負荷状態となって先端リンクの現在値ベクトルを目標値ベクトルに完全に追従させることが困難となった場合でも、目標値ベクトルのうち重要な要素については高い追従性が維持されるように非過負荷関節を制御することができる。これにより、ある関節が過負荷状態となってもできるだけ作業を継続することのできる多関節ロボットが実現できる。本実施例の多関節ロボットは、どの関節が過負荷となっても、目標値ベクトルの要素のうち、常に高順位の要素について追従性が確保される。本実施例の多関節ロボットによれば、どの関節が過負荷関節となっても、予定されていた作業を可能な限り継続できる。
なお、本実施例では、目標関節角が目標駆動量の一態様に相当する。図2の負荷推定部32が請求項の負荷推定部の一態様に相当する。過負荷関節特定部34が請求項の過負荷関節特定部の一態様に相当する。力制御演算部36が請求項の第1駆動量算出部の一態様に相当する。目標値要素抽出部42と逆変換部44が請求項の第2駆動量算出部の一態様に相当する。
本実施例では、負荷推定部32、過負荷関節特定部34、力制御演算部36、目標値要素抽出部42、及び逆変換部44はコントローラ30のCPUが実行するコンピュータプログラムモジュールとして実装される。負荷推定部32等は、夫々専用の処理を行う専用ハードウエアで実装されてもよい。
【0033】
<第2実施例> 次に第2実施例について説明する。図4に第2実施例の多関節ロボットのブロック図を示す。図4では、図2に示す第1実施例のブロック図の構成要素と同じ構成要素については同じ符号を付してある。図2と同じ符号を付したブロックについては説明を省略する。
第2実施例の多関節ロボットにおけるロボット本体10bは、先端リンク(図1の第7リンク12g)に力センサ28を有する。それ以外は第1実施例のロボット本体10と同じであるので説明を省略する。力センサ28は、手先効果器14(図1参照)とロボット本体10bの先端リンクの間に取り付けられている。即ち、力センサ28は手先効果器14を介してロボット本体10bに加わる外力を検出する。
【0034】
第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は想定外外力に起因する想定外負荷を推定するので、閾値は想定外負荷を許容できる値に設定される。
本実施例の多関節ロボットは、手先効果器以外の部位で他の物体と接触した場合に、その接触を解消しながら、接触の解消に関与しない関節(即ち非過負荷関節)は、できるだけ先端リンクの現在値ベクトルを目標値ベクトルに追従させるように制御される。即ち、本実施例の多関節ロボットは、手先効果器以外の部位で他の物体と接触した場合に、その接触を解消しながら、ロボットが実行中の作業を可能な限り継続することができる。
【0035】
<第3実施例> 次に本発明の第3実施例について説明する。第1実施例と第2実施例では、非過負荷関節の目標関節角を決定する際に逆変換を用いた。第3実施例では、各関節を制御する際の目標値(目標駆動量)は関節角速度であり、目標値ベクトルは先端リンクの目標速度(図1の座標系におけるx、y、z各軸方向の速度及び、ロール角速度、ピッチ角速度、ヨー角速度)で表される。即ち、過負荷関節が存在しない場合には第3実施例の多関節ロボットはいわゆる分解速度制御が行なわれる。本実施例は、非過負荷関節の目標値(角速度)を決定する際に分解速度制御を応用する。なお、本実施例では、多関節ロボットのブロック図は図2を参照する。但し図2における逆変換部44は、分解速度制御演算部に置き換える。
【0036】
先端リンクの目標値ベクトルから目標関節角速度ベクトル(関節の目標駆動量ベクトル)へはヤコビ行列を用いて変換できる。目標値ベクトルをdXで表し、各関節の現在関節角を要素とする関節角ベクトルをΘで表し、各関節の目標角速度を要素とする目標関節角速度ベクトル(目標駆動量ベクトル)をdΘで表し、ヤコビ行列をJ(Θ)で表すと、それらの関係は次の(3)式で表せる。
dX=J(Θ)・dΘ ・・・(3)
従って、目標関節角速度ベクトルdΘは次の(4)式で表せる。
dΘ=J−1(Θ)・dX ・・・(4)
ここでJ−1(Θ)はヤコビ行列J(Θ)の逆行列である。第1実施例と同じく、多関節ロボットは6個の関節を有しており、目標値ベクトルは先端リンクのx軸、y軸、z軸方向の速度と、3軸夫々の軸周りのロール角速度、ピッチ角速度、及びヨー角速度を要素とする6次元ベクトルである。(3)式において、dXとdΘが共に6次元であるのでヤコビ行列J(Θ)は6×6の正方行列となる。従って、ヤコビ行列J(Θ)の行列式がゼロでない限り逆行列J−1(Θ)が存在して(4)式によって目標値ベクトルdXからdΘを求めることができる。各要素が(角)速度の単位を有する目標値ベクトルに対して、ヤコビ行列の逆行列を用いて目標関節角速度を算出し、得られた目標関節角速度を実現するように各関節のアクチュエータを制御する方法が分解速度制御法と呼ばれる制御法である。
【0037】
本実施例において、過負荷関節が存在する場合の処理を説明する。今、図2の過負荷特定部34によって、6次元の関節角ベクトルΘの第i要素の関節が過負荷関節と特定されたとする。過負荷関節がひとつ存在するので、目標値要素抽出部42は6次元の目標値ベクトルdXの要素のうち優先順位の上位から5個の要素を抽出する。換言すれば、5次元に低次元化した目標値ベクトルを抽出する。ここで、6次元の目標値ベクトルdXの要素のうち、第j要素が最も優先順位の低い要素とする。本実施例の分解速度制御演算部は、第j要素の欠けた5次元の目標値ベクトル(dX’と表す)から第i要素の欠けた5次元の目標関節角速度ベクトル(dΘ’と表す)を決定する。(3)式に示した6×6のヤコビ行列J(Θ)から第j行と第i列を除外した5×5の低次元ヤコビ行列をJ’(Θ)と表すと、次の(5)式が成立することがわかる。
dX’=J’(Θ)・dΘ’ ・・・(5)
従って、5次元の目標関節角速度ベクトルdΘ’は次の(6)式で求められる。
dΘ’=J’−1(Θ)・dX’ ・・・(6)
本実施例の分解速度制御演算部は、(6)式によって非過負荷関節(第i番目の関節を除く関節)の目標関節角速度ベクトルdΘ’を決定することができる。決定された目標関節角速度ベクトルdΘ’の各要素(各非過負荷関節の目標関節角速度)はモータドライバ38に送られる。モータドライバ38は目標関節角速度に追従するようにアクチュエータ22を制御する。なお、本実施例における力制御演算部36は、駆動軸ごとにインピーダンス制御或いは力制御の制御則に基づいて、過負荷関節の目標関節角速度を算出する。
なお、上記例では、過負荷関節はひとつと仮定したが、過負荷関節が複数存在する場合も同様の議論が成立する。
【0038】
第3実施例では、分解速度制御法を利用して非過負荷関節の目標関節角速度(目標駆動量)を算出する。分解速度制御はヤコビ行列の逆行列を用いて速度の単位を有する要素からなる目標値ベクトルから各関節の目標関節角速度を決定する方法である。第3実施例では、多関節ロボットのヤコビ行列を設定し、そのヤコビ行列において、特定された過負荷関節に対応する列(上記例では第i列)と、抽出されなかった目標値ベクトルの要素に対応する行(上記例では第j行)を除外した低次元ヤコビ行列を特定する。その低次元ヤコビ行列の逆行列を用いて抽出された目標値ベクトルの要素から非過負荷関節の目標関節角速度(目標駆動量)を決定する。第3実施例では、目標値ベクトルへの追従制御に利用できる関節(非過負荷関節)の数を特定する。そして目標値ベクトルについて、その要素の数を非過負荷関節の数と同数に低次元化する。非過負荷関節の数と目標値ベクトルの次元を一致させることによって、それらの間に成立するヤコビ行列を常に正方行列に維持することができる。ヤコビ行列が正方行列に維持されるので、ヤコビ行列の逆行列を算出することができる(但し特異点は除く)。従って、先端リンクの現在値ベクトルを低次元化した目標値ベクトルに追従させる目標駆動量を、低次元化したヤコビ行列によって簡単に決定することができる。本実施例における分解速度制御演算部(図2或いは図4において逆変換部44と置き換えられる)と図2或いは図4の目標値要素抽出部42が、請求項の第2駆動量算出部の一態様に相当する。
【0039】
以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
【0040】
例えば、各駆動軸にはロボットの自重に起因する負荷も作用する。或いは、ロボットが高速に動作する場合には慣性力に起因する負荷も作用する。ロボットの自重や慣性力による負荷は予想される負荷であり、過負荷関節を特定する際に閾値と比較する推定負荷から除外した方がよい場合もある。そのような場合には、負荷推定部は、関節ごとに、駆動軸に作用する総負荷から重力に起因する負荷を除外した負荷の大きさと方向を推定することも好適である。或いは、負荷推定部は、関節ごとに、駆動軸に作用する総負荷から慣性力に起因する負荷を除外した負荷の大きさと方向を推定することも好適である。さらに負荷推定部は、関節ごとに、駆動軸に作用する総負荷から、重力と慣性力の双方に起因する負荷を除く負荷の大きさと方向を推定することも好適である。なお、ロボットの自重に起因して駆動軸に作用する負荷の大きさと方向は、ロボットの各部の質量と慣性モーメントと各関節の現在関節角によって算出できる。慣性力に起因して駆動軸に作用する負荷の大きさと方向は、さらに各関節の関節角加速度と角速度によって算出できる。
【0041】
本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【図面の簡単な説明】
【0042】
【図1】第1実施例の多関節ロボットの概要図である。
【図2】第1実施例の多関節ロボットのブロック図である。
【図3】第1実施例の多関節ロボットの制御フローチャート図である。
【図4】第2実施例の多関節ロボットのブロック図である。
【符号の説明】
【0043】
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:逆変換部
【出願人】 【識別番号】000003207
【氏名又は名称】トヨタ自動車株式会社
【出願日】 平成18年6月26日(2006.6.26)
【代理人】 【識別番号】110000110
【氏名又は名称】特許業務法人快友国際特許事務所


【公開番号】 特開2008−861(P2008−861A)
【公開日】 平成20年1月10日(2008.1.10)
【出願番号】 特願2006−175063(P2006−175063)