JP4440072B2

JP4440072B2 - ロボットの制御方法

Info

Publication number: JP4440072B2
Application number: JP2004310506A
Authority: JP
Inventors: 直人増永; 広之中田; 康士向井; 達也池田
Original assignee: Panasonic Corp; Matsushita Electric Industrial Co Ltd
Current assignee: Panasonic Corp; Panasonic Holdings Corp
Priority date: 2004-10-26
Filing date: 2004-10-26
Publication date: 2010-03-24
Anticipated expiration: 2024-10-26
Also published as: JP2006123012A

Description

本発明は、モータにより駆動されるロボットの衝突を検出する制御方法に関する。

近年、ロボットにおいて、ワークなどへの衝突時の安全性向上や破壊による損失防止のため、衝突検出の高精度化が求められている。しかしながら、高精度な衝突センサを用いることはコストが増大し、さらにロボットのアームに設けたセンサの重量が負荷として働くので、ロボットの高速化や省エネに反することになる。

そこで、センサレスで衝突力を高精度に検出する制御方法が求められている。センサレスで高精度に衝突力を求める方法としては、逆動力学演算により得られる、負荷が装着されたアームを動作させるために必要なモータ駆動電流の計算値と、フィードバック制御によりモータに流される実電流値との差分である外乱電流値を計算し、外乱電流値が予め設定した閾値より大きければアームが外力を受け衝突したと判断する制御方法が知られている（例えば、非特許文献１参照）。

逆動力学演算により得られるモータ駆動電流の計算値の中でも、重力成分は大きな比重を占める。この重力成分を計算するためには、アームとアームに装着される負荷の質量と重心位置を必要とする。

このうち、ロボットが備えるアームの質量と重心位置は、ロボットの設計時に使用されたＣＡＤ等の設計ツールによって予め同定することが可能である。しかし、ロボットのアームへ任意の負荷が装着される場合では、装着負荷の質量と重心位置の負荷パラメータ情報が必要となる。

従来のロボットの制御方法においては、ロボット装着負荷の質量と重心位置の負荷パラメータ情報を得る手段としては、ロボット操作者がコントローラへ負荷パラメータの数値入力を行うことが一般的である。
小菅一弘、外1名、"マニピュレータの動的衝突検出"、日本機械学会ロボティクス・メカトロニクス講演会'９９講演論文集２Ａ１-１１−０３０（１９９９年）

しかしながら、ロボット操作者がコントローラへ負荷パラメータの数値入力を行う従来の方法では、以下のような問題が発生する。

ここで、従来の方法の問題を説明する前に、まず、センサレスの衝突力検出手段を含むロボットの制御ブロックについて、図２を用いて説明する。

なお、ロボット全体としては図２と同様のブロック図が複数軸分存在するが、図２を代表例としてモータ１軸分の制御について示す。例えば、図３に示す６軸多関節ロボットでは、図２と同様の制御方法を６個持つ。

図３は６軸多関節ロボットでのアーム６に装着された負荷の質量と負荷重心位置１５を示した一例である。

図３において、負荷重心位置１５のＸ軸方向の位置がＬＸ２０、Ｙ軸方向の位置がＬＹ２１とＺ軸方向の位置がＬＺ２２である。負荷の質量１４と負荷の重心位置ＬＸ２０、ＬＹ２１、ＬＺ２２とは、ロボットの仕様により、上限値や下限値等の設定範囲が決まっている。

図２に示すように、ロボット制御ブロックは、衝突を検出する制御方法を実現するためのコントローラ１と、パラメータ入力手段７と、モータ５と、モータ５により駆動されるアーム６から構成されている。

なお、パラメータ入力手段７は、負荷の質量１４と、重心位置１５の負荷パラメータ（Ｘ，Ｙ，Ｚ方向の重心位置）と、衝突検出閾値Ｉｔｈ１６をコントローラ１へ入力する手段である。

また、コントローラ１に内蔵されたフィードバック（以下ＦＢと略す）制御器３は、位置指令θｃｏｍ２とモータ回転角θｆｂ８とに基づいて電流指令Ｉｃｏｍ４を発生させ、モータ５を制御する。

また、コントローラ１は、モータ５から、アーム６駆動時のモータＦＢ電流値Ｉｆｂ９とモータ回転角θｆｂ８を得ることができる。

一方、コントローラ１内の駆動モータ電流算出１０は、モータ回転角θｆｂ８と、パラメータ入力手段７により入力されたアーム６へ装着される負荷の質量１４および重心位置１５とから、逆動力学演算を行うことにより、モータ駆動に必要なモータ電流計算値Iｃａｌ１１を求める。

なお、厳密にいえば、図３の多関節ロボットのように複数軸のモータが同時に駆動する場合では、駆動モータ電流算出１０は、自軸以外の駆動軸の干渉トルク換算電流値と重力トルク換算電流値の変化量を計算するために、駆動軸分のモータ回転角θｆｂｉ１８を逐次必要とする。

しかし、ここではモータ電流計算値値Iｃａｌ１１の内、重力成分のみを取り扱うので、説明をわかりやすくするため、複数軸存在する中の１軸のみが駆動される状態とし、かつ、運転前の他駆動軸のモータ回転角θｆｂｉ１８は既知と仮定することにより、モータ駆動に必要なモータ電流計算値Iｃａｌ１１が自軸動作の重力成分として取り扱える場合を例に挙げて説明する。

さて、コントローラ１に内蔵された衝突検出手段１７では、モータ駆動に必要なモータ電流計算値Iｃａｌ１１と、モータ５から得られるモータＦＢ電流値Ｉｆｂ９との差を、衝突により発生する外乱電流値Ｉｄｉｓ１２として計算する。

アーム６へ装着される負荷の質量１４と重心位置１５とが正しく設定されれば、モータ電流計算値Iｃａｌ１１は、モータ５がアーム６を駆動するためだけに必要とするモータＦＢ電流値Ｉｆｂ９の理想値と等しくなり、衝突検出判定１３は、外乱電流値Ｉｄｉｓ１２がパラメータ入力手段７用いて予め設定した衝突検出閾値Ｉｔｈ１６より大きければアーム６が外力を受けた、すなわち、衝突したと判断することができる。

アーム６へ装着される負荷の質量１４と重心位置１５とが正しく設定された場合のロボットの運転の一例を図４に示す。なお、図４においては、図５に示すように、負荷の質量１４と重心位置ＬＸ２０、ＬＹ２１、ＬＺ２２とが設定範囲の最大値である負荷が装着されているとする。

まず、衝突が発生しない場合について説明する。

図４は、第６軸の運転における、衝突が発生しない場合のモータＦＢ電流Ｉｆｂ９の一例である。図４においては、負荷パラメータが正しく入力され、衝突が発生していないため、モータ電流計算値Iｃａｌ１１とモータＦＢ電流値Ｉｆｂ９とはほぼ一致しており、外乱電流値Ｉｄｉｓ１２は、ほぼ０付近で推移している。そして、外乱電流値Ｉｄｉｓ１２は衝突検出閾値Ｉｔｈ１６より小さいので、衝突は検知されない。

図６は、負荷パラメータが正しく設定された図４の運転における、ＬＺ２２の実際に装着されている位置と、設定された位置と、これらの計算誤差ｄ３１とを示した図である。ｄ３１は電流値とした場合の差を示している。実際に装着されている位置と、設定された位置とが一致しているため、計算誤算ｄ３１はゼロとなる。

次に、衝突が発生した場合について説明する。

図７は、図４の場合と同様の運転を行っているが、衝突発生時間ｔ０２３において衝突が発生した場合のモータＦＢ電流値Ｉｆｂ９の一例である。なお、図７に示す衝突発生時間ｔ０２３で衝突が発生する場合を示している。

図７においては、衝突が発生するｔ０２３迄の区間では、図４と同様に、モータ電流計算値Iｃａｌ１１とモータＦＢ電流値Ｉｆｂ９はほぼ一致しており、外乱電流値Ｉｄｉｓ１２は、ほぼ０付近で推移している。

しかし、ｔ０２３で衝突が発生すると、モータＦＢ電流値Ｉｆｂ９は衝突トルクに対抗するために増加し、モータ電流計算値Iｃａｌ１１とは乖離するため、その差である外乱電流値Ｉｄｉｓ１２も増加する。

そして、衝突検出時間ｔ１２４で、外乱電流値Ｉｄｉｓ１２が衝突検出閾値Ｉｔｈ１６を越えると衝突したと判断される。

なお、図７においては、衝突検出閾値Ｉｔｈ１６は最大モータ電流値比３０％に設定した例を説明しているが、衝突が発生するｔ０２３迄の区間での外乱電流値Ｉｄｉｓ１２の小ささを考慮すると、もっと低い値での設定も可能である。

しかし、ロボットの運転では、負荷パラメータの質量１４と重心位置１５の値が、実際にロボットへ装着された負荷のそれと異なる場合がある。なお、この様なことが発生する場合としては、ロボットの操作者が負荷パラメータを設定しない場合や、設定する値を間違う場合が考えられる。

そして、ロボットの操作者が負荷パラメータを設定しない理由としては、ロボットの操作者の設定忘れや、衝突検出閾値を敢えて低くする（衝突検出感度を高くする）必要がなく、負荷パラメータ設定の必要性を感じない場合などが考えられる。

また、ロボットの操作者が負荷パラメータを設定した場合でも、計算ミス等が原因でコントローラ１へ設定する値が間違っている場合も考えられる。

コントローラ１へ設定する値の中でも、質量１４は、質量計を用いれば比較的容易に測定が可能であり、設定の間違いを招きにくい。

一方、重心位置１５は、一般的には計算によって求める。しかし、ロボットに装着された負荷が複数のパーツで構成されている場合や、複雑な形状を持つ場合では、個々のパーツや単純な形状部分ごとに重心位置を算出し、これらを組合せて再度全体の重心位置を算出しなければならない。そのため、計算が複雑で、かつ操作者の手を煩わすこととなり、設定が容易に出来ず、設定の間違いを招きやすい。

また、重心位置の計算が容易であっても、設定を間違う場合がある。例えば、モータ回転角θｆｂ８がオフセットを持っている状態において負荷を装着する場合や、負荷を装着する方向が異なる場合でも、負荷の重心位置１５の設定は異なる。

したがって、負荷の質量１４の設定に比べると、負荷の重心位置の設定は難しく、操作者の設定の間違いを招きやすい。

以上のようなことに起因して、設定された負荷の重心位置１５が、実際にアーム６へ装着された負荷の重心位置１５とは異なる場合がある。

設定された負荷の重心位置１５が、実際にアーム６へ装着された負荷の重心位置１５とは異なって設定される場合では、モータ電流計算値Iｃａｌ１１は駆動モータ電流算出１０の計算誤差を含み、モータＦＢ電流値Ｉｆｂ９の理想値とはならない。

特に、負荷の重心位置１５の符号が反転して設定された場合では、正確に負荷の重心位置１５が設定されたモータ電流計算値Iｃａｌ１１に対して、符号が反転したモータ電流計算値Iｃａｌ１１が計算され、モータＦＢ電流値Ｉｆｂ９の理想値とはならず非常に大きな計算誤差ｄ３１を持つ。

図８は、図５で示した同じ負荷を反転して取り付けた場合を示す。なお、図８では、第６軸の運動に影響する重心位置のＺ方向成分であるＬＺ２２のみが反転した例を示している。

図９は、図８に示すＬＺ２２の実際に装着されている位置と、設定された位置と、これら電流とした場合の計算誤差ｄ３１とを示した図である。実際に装着されている負荷の位置はＬＺｍａｘ２７であるが、設定された位置はＬＺｍｉｎ２８であるため、これらの差ｄ３１はｄｍａｘ３２と最大になっている。

図８の場合において、図１０に、アーム６へ装着される負荷の重心位置１５が反転して設定された場合の外乱電流値Ｉｄｉｓ１２の一例を示す。

図１０において、外乱電流値Ｉｄｉｓ１２は、負荷の重心位置１５の符号が異なることにより、正しい重心位置１５が設定された外乱電流値Ｉｄｉｓ１２に対して、モータ電流計算値Iｃａｌ１１とモータＦＢ電流Ｉｆｂ９が重畳された非常に大きな値となる。

したがって、実際のロボットの運転では衝突していないにも関わらず、予め設定した衝突検出閾値Ｉｔｈ１６より外乱電流Ｉｄｉｓ１２が大きくなる衝突発生時間ｔ１２４にて衝突を誤検出してしまう。

そして、衝突の誤検出が発生した場合には、ロボットはエラー状態で一時停止してしまう。この場合、ロボットの操作者は、誤検出を防止するために衝突誤検出が発生しないレベルにまで、衝突検出閾値Ｉｔｈ１６を上げなければならなくなる。つまり、負荷の重心位置１５の設定に誤差があるために、衝突検出閾値Ｉｔｈ１６を高くせざるを得ない。

図１０の運転で誤検出を防ぐためには、衝突検出閾値Ｉｔｈ１６を最大モータ電流値比を例えば９０％まで上げなければならない。

図１１は衝突検出閾値Ｉｔｈ１６を最大モータ電流値比９０％として設定した場合の外乱電流値Ｉｄｉｓ１２の一例である。

図１１では、衝突検出閾値Ｉｔｈ１６を最大モータ電流値比９０％として設定したため、衝突の誤検出は発生していない。しかし、衝突検出閾値Ｉｔｈ１６を上げたことより、時刻ｔ０２３にて実際に発生した衝突の検出を行うことができない。

上記課題を解決するために、本発明の多関節型ロボットの制御方法は、モータで駆動するアームを複数有する多関節型ロボットにおいて、予め記憶された前記アームの質量および重心位置とユーザにより設定されて予め記憶された前記アームに装着された負荷の質量および重心位置と前記モータの回転位置情報とから前記モータの駆動に必要な電流として逆動力学演算により得られる電流計算値と、計測によって得られる前記モータの実電流値との差分を計算し、前記差分が予めユーザにより設定された衝突検出閾値より大きければアームが外力を受けたと判断する多関節型ロボットの制御方法であって、前記負荷の重心位置はロボットの最先端の回転軸と前記負荷を取り付ける面との交点を原点とした座標系における、原点と重心との各軸方向の距離であり、ユーザにより設定された衝突検出閾値が、予め設定された衝突検出閾上値以上であればユーザが設定した負荷の重心位置を原点位置である０の位置に変更し、予め設定された衝突検出閾下値以下であればユーザが設定した負荷の重心位置を変更せず、予め設定された衝突検出閾上値より小さく予め設定された衝突検出閾下値より大きければ衝突検出閾値の大きさに比例するようにユーザが設定した負荷の重心位置を変更し、この衝突検出閾値の大きさに応じて変更された負荷の重心位置を用いて前記電流計算値を算出して前記アームが外力を受けたか否かを判断するものである。

本発明のロボット制御方法は、ロボットの操作者により設定される負荷重心位置を予め設定される所定の閾値に応じて変更可能とし、電流計算値における重力成分の計算に用いることによって、ロボット動作時の衝突誤検出を防止することができる。

以下、実施の形態のロボットの制御方法について、図面を参照しながら説明する。

（実施の形態１）
本実施の形態が適用されるロボットの制御方法の基本的な部分は、図２を用いて説明した従来の制御方法と同様であり、後述する図１に示すオフセット感度Ａ１９を設けた点が従来の制御方法と異なる点である。

なお、実施の形態において、発明が解決しようとする課題で説明したものと同一のものについては同一の符号を付して詳細な説明を省略する。

従来の制御方法において、コントローラ１が衝突の誤検出を起こす要因は、間違って設定された負荷の重心位置１５の設定を用いてモータ電流計算値Iｃａｌ１１の重力成分を計算することで、正しく設定されたときのモータ電流計算値Iｃａｌ１１との間に大きな計算誤差ｄ３１が発生していることである。

負荷の重心位置１５に着目すると、負荷の重心位置１５はロボットの仕様によって上限値や下限値等の設定範囲が定められている。図５に示すような重心位置１５のＺ軸方向の位置ＬＺ２２を例として挙げると、位置ＬＺ２２の設定値は、図６に示すような設定範囲として、上限値ＬＺｍａｘ２７と下限値ＬＺｍｉｎ２８を持ち、上限値ＬＺｍａｘ２７と下限値ＬＺｍｉｎ２８の間で任意に設定される。

いま、装着される負荷のＺ軸方向の重心位置１５が上限値ＬＺｍａｘ２７または下限値ＬＺｍｉｎ２８となる時、設定される重心位置１５およびモータ電流計算値Iｃａｌ１１の計算誤差ｄ３１との関係は図６や図９に示すとおりである。詳細については後述する。

そして、重心位置１５の設定範囲内で、設定位置が、図６のように設定されることもあれば図９のように設定されることもあり、負荷の重心位置１５がどこに設定されるか特定できない。しかし、負荷の重心位置１５は必ず上限値と下限値の設定範囲内に設定される。したがって、図９に示すように、実際の電流と計算による電流との計算誤差ｄ３１が最大誤差ｄｍａｘ３２となることを防ぐために、図１２に示すように、設定位置を、負荷の重心位置１５の上限値と下限値との中間値ＬＺ０と設定すれば、常にモータ電流計算値Iｃａｌ１１の計算誤差ｄ３１を最大誤差ｄｍａｘ３２の半分以内にすることができる。

なお、重心位置１５のＺ軸方向の位置ＬＺ２０を例に挙げたが、位置ＬＸ２１ならびに位置ＬＹ２２についても、設定値は上限値から下限値までの設定範囲を持ち、位置ＬＺ２０と同様の特徴を持つ。よって、図９に示すように、計算誤差ｄ３１が最大誤差ｄｍａｘ３２に近くなる場合では、重心位置１５を重心位置１５の上限値と下限値の中間値として再設定すれば、計算誤差ｄ３１を最大誤差ｄｍａｘ３２の半分以内にすることができる。

しかし、図６に示すように、負荷の重心位置１５が正しく設定される場合では、計算誤差３１がほぼ０となるので、重心位置１５を中間値に再設定すると、計算誤差３１が最大誤差ｄｍａｘ３２の半分以下となるので、再設定によって計算誤差３１を大きくしてしまう。したがって、負荷の重心位置１５が正しく設定される場合とそうでない場合とで、再設定の方法を使い分けなければならない。

ロボットの運転において、重心位置１５が正しく設定される場合では、図４で示したように外乱電流値Ｉｄｉｓ１２が小さいので、衝突検出閾値Ｉｔｈ１６を敢えて低く設定する用途があり、反対に重心位置１５の設定が実際にロボットへ装着された負荷と異なる場合では、外乱電流値Ｉｄｉｓ１２が大きくなるので、衝突検出閾値Ｉｔｈ１６が高く設定される。したがって、衝突検出閾値Ｉｔｈ１６を参照することで再設定の方法を使い分けることができる。

しかし、設定方法を使い分けることは、衝突検出閾値Ｉｔｈ１６の設定によって計算誤差ｄ３１の不連続点を作り、ある設定値を境界として誤検出を招くこととなるので、衝突検出閾値Ｉｔｈ１６の設定を難しくしてしまう。

そこで、衝突検出閾値Ｉｔｈ１６が予め設定された衝突検出閾上値Ｉｍａｘ３３以上である場合には、重心位置１５を、例えば、図１２を用いて説明したような重心位置１５の設定範囲の中間値と設定する。また、衝突検出閾値Ｉｔｈ１６が予め設定された衝突検出閾下値Ｉｍｉｎ３４以下である場合には、設定どおりの負荷の重心位置１５と設定する。また、衝突検出閾値Ｉｔｈ１６が衝突検出閾上値Ｉｍａｘ３３より小さく衝突検出閾下値Ｉｍｉｎ３４より大きい場合には、衝突検出閾値Ｉｔｈ１６に比例して負荷の重心位置１５を中間値に近づけるように設定する。

なお、衝突検出閾上値Ｉｍａｘ３３と衝突検出閾下値Ｉｍｉｎ３４は任意に設定することができるものであるが、一般的に、ロボットの仕様等により決まるものである。

また、図１３は、重心位置１５の設定範囲が上限値と下限値の絶対値が等しくなる場合におけるオフセット感度Ａの設定値の一例を示すものであり、衝突検出閾値Ｉｔｈ１６が衝突検出閾上値Ｉｍａｘ３３より小さく衝突検出閾下値Ｉｍｉｎ３４より大きい区間を一次近似している。

次に、上記したような重心位置１５の再設定を行う本実施の形態の制御方法について説明する。

図１に、従来の制御方法に、上記で説明したオフセット感度Ａを用いてパラメータ入力手段７により入力された重心位置１５を再設定するためのオフセット感度Ａ１９を加えた本実施の形態の制御方法のブロック図を示す。

図１４は、図１に示すブロック図の構成による制御方法により、図１１に示した運転と同様の負荷パラメータの設定を行って運転した場合のモータ電流計算値Ｉｃａｌ１１と外乱電流値Ｉｄｉｓ１２の一例を示す図である。

なお、図１１に示した運転と同様の負荷パラメータの設定を行って運転しているため、当初、衝突検出閾値Ｉｔｈ１６は、上記したように、衝突誤検出とならないように、図１１に示したのと同様、最大モータ電流値比９０％に設定されているものとする。

そして、本実施の形態による制御方法では、衝突検出閾値Ｉｔｈ１６が最大モータ電流値比９０％と大きいので、重心位置１５が小さくなるように重心位置１５を再設定するためのオフセット感度Ａを、図１３に基づいてコントローラ１内の図示しない演算部で演算し、求めたオフセット感度Ａをオフセット感度Ａ１９に出力する。そして、パラメータ入力手段７により入力された重心位置１５は、オフセット感度Ａ１９により再設定され、駆動モータ電流算出１０に出力される。これにより、この再設定された重心位置１５に基づいて算出されるモータ電流計算値Ｉｃａｌ１１とモータＦＢ電流Ｉｆｂ９との差である外乱電流Ｉｄｉｓ１２が小さくなり、操作者は、衝突検出閾値Ｉｔｈ１６を最大モータ電流値比９０％から下げて設定することができるようになる。図１５では、衝突検出閾値Ｉｔｈ１６を最大モータ電流値比６０％に下げて設定した例を示している。

なお、この設定は、例えば、操作者が最大モータ電流値比９０％から段階的に低減していき、衝突誤検出を生じないレベルを実際に確認する作業を行うことで実現される。

そして、図１５は、ロボット運転中に衝突が発生した場合における、モータ電流計算値Iｃａｌ１１と外乱電流値Ｉｄｉｓ１２の一例も示している。
図１５に示すように、衝突の誤検出は発生せず、ロボット運転中の衝突発生時刻ｔ０で発生した衝突を衝突検出時間ｔ１２４にて検出することができる。

以上説明したように、ロボットの操作者により予め設定された衝突検出閾値Ｉｔｈ１６に比例させて、ロボットの操作者により設定された負荷の重心位置１５を駆動モータ電流算出１０に再設定し、外乱電流値Ｉｄｉｓ１２を算出することにより、外乱電流値Ｉｄｉｓ１２を小さくすることができる。すなわち、重心位置１５の誤った設定による計算誤差を低減することができる。そして、外乱電流値Ｉｄｉｓ１２が小さくなることで、衝突検出閾値Ｉｔｈ１６を低く設定することができる。

本来、重心位置１５が正しく設定される場合は、外乱電流値Ｉｄｉｓ１２が小さいので、衝突検出閾値Ｉｔｈ１６も低く設定される。すなわち、衝突検出閾値Ｉｔｈ１６は低く設定されることが望ましい。

以上により、本実施の形態の制御方法によれば、必要以上に衝突検出閾値Ｉｔｈ１６を上げることなく、ロボット運転時の衝突誤検出を防止するとともに、衝突の検知を行うことができる。

本発明は、ロボットの操作者により入力される負荷の重心位置を予め設定される所定の閾値に比例させてモータの駆動電流の重力成分を計算することにより、ロボット運転時の衝突を検出する制御方法を提供するものであり、検出手段が必要とする装着負荷の質量と重心位置の設定忘れや設定値の計算ミスや座標方向を考慮した符号の入力ミスによって発生する衝突誤検出の防止しすることができ、モータにより駆動されるロボットの衝突を検出する分野等に利用することができる。

本発明の実施の形態1における制御方法を実施するためのブロック図従来の制御方法を実施するためのブロック図（ａ）ロボットを構成する駆動軸の配置の一例を示す側面図（ｂ）ロボットを構成する駆動軸の配置の一例を示す正面図モータＦＢ電流の特性の一例を示す図（ａ）ロボット第６軸に装着された負荷の座標の一例を示す側面図（ｂ）ロボット第６軸に装着された負荷の座標の一例を示す正面図負荷の重心位置の設定と計算誤差の相関の特性の一例を示す図図４の運転時に衝突による外力が加わった場合における外乱電流値の特性の一例を示す図（ａ）ロボット第６軸に装着された負荷の座標の異なる一例を示す側面図（ｂ）ロボット第６軸に装着された負荷の座標の異なる一例を示す正面図負荷の重心位置の設定と計算誤差の相関の特性の異なる一例を示す図図４と同じ運転を行った外乱電流値の特性の異なる一例を示す図図１０の運転時に衝突による外力が加わった場合における外乱電流値の特性の一例を示す図負荷の重心位置の設定計算誤差の相関の特性の一例を示す図オフセット感度の設定値の一例を示す図運転時に衝突による外力が加わった場合における外乱電流値の特性の一例を示す図

符号の説明

Ｔｍモータトルク
ＬＸ重心位置１５のＸ軸方向の位置
ＬＹ重心位置１５のＹ軸方向の位置
ＬＺ重心位置１５のＺ軸方向の位置

Claims

モータで駆動するアームを複数有する多関節型ロボットにおいて、予め記憶された前記アームの質量および重心位置とユーザにより設定されて予め記憶された前記アームに装着された負荷の質量および重心位置と前記モータの回転位置情報とから前記モータの駆動に必要な電流として逆動力学演算により得られる電流計算値と、計測によって得られる前記モータの実電流値との差分を計算し、前記差分が予めユーザにより設定された衝突検出閾値より大きければアームが外力を受けたと判断する多関節型ロボットの制御方法であって、前記負荷の重心位置はロボットの最先端の回転軸と前記負荷を取り付ける面との交点を原点とした座標系における、原点と重心との各軸方向の距離であり、ユーザにより設定された衝突検出閾値が、予め設定された衝突検出閾上値以上であればユーザが設定した負荷の重心位置を原点位置である０の位置に変更し、予め設定された衝突検出閾下値以下であればユーザが設定した負荷の重心位置を変更せず、予め設定された衝突検出閾上値より小さく予め設定された衝突検出閾下値より大きければ衝突検出閾値の大きさに比例するようにユーザが設定した負荷の重心位置を変更し、この衝突検出閾値の大きさに応じて変更された負荷の重心位置を用いて前記電流計算値を算出して前記アームが外力を受けたか否かを判断する多関節型ロボットの制御方法。