JP4756618B2 - Collision detection / stop control method for articulated robots - Google Patents

Collision detection / stop control method for articulated robots Download PDF

Info

Publication number
JP4756618B2
JP4756618B2 JP2001081553A JP2001081553A JP4756618B2 JP 4756618 B2 JP4756618 B2 JP 4756618B2 JP 2001081553 A JP2001081553 A JP 2001081553A JP 2001081553 A JP2001081553 A JP 2001081553A JP 4756618 B2 JP4756618 B2 JP 4756618B2
Authority
JP
Japan
Prior art keywords
joint
torque
robot
collision
speed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
JP2001081553A
Other languages
Japanese (ja)
Other versions
JP2002283276A (en
Inventor
泰宏 神品
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Daihen Corp
Original Assignee
Daihen Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Daihen Corp filed Critical Daihen Corp
Priority to JP2001081553A priority Critical patent/JP4756618B2/en
Publication of JP2002283276A publication Critical patent/JP2002283276A/en
Application granted granted Critical
Publication of JP4756618B2 publication Critical patent/JP4756618B2/en
Anticipated expiration legal-status Critical
Expired - Fee Related legal-status Critical Current

Links

Landscapes

  • Numerical Control (AREA)
  • Manipulator (AREA)

Description

【0001】
【発明の属する技術分野】
本発明は多関節ロボットにおける衝突検出・停止制御法に係り、詳しくは、ロボットが稼働中にワーク等の外界の対象物や障害物等に衝突すれば、それをいち早くかつ精度よく検出し、その後速やかにロボットの動きを止めたり力を解放して、ロボット本体やツールの変形,損傷を回避できるようにしたロボットの制御法に関するものである。
【0002】
【従来の技術】
複数の関節からなるロボットのリンクやロボットの先端に把持されたツールもしくはワークが、教示データに基づいて再生動作しているときやティーチペンダントによる教示操作で動いているとき、意図することなく加工対象物や周辺の障害物に当たることがある。再生動作中であれば、ロボットの各関節を駆動するそれぞれのモータは指令値の通りに、また教示中でも衝突に気づかず教示を続ければ、ロボットは当然に障害物等を突破してでも動こうとする。その結果、関節によっては過大なトルクが発生し、それに伴ってリンクやツールが変形したり関節等のロボットの可動部を損傷させるといった事態を招く。
【0003】
ロボットに教示された作業データの再生動作を実行させる各モータを制御するためのそれぞれのサーボドライバには、例えば図14に示すように、位置や速度の制御系が組み込まれ、図示しない制御装置(ロボットコントローラ)から与えられる位置指令にモータ1の動作を追従制御させ、作業データの再生動作が実行されるようにしている。また、各サーボドライバ2には速度制御系の後にトルク制限器3が配置され、速度制御によって生成されるトルク指令を任意の値で制限しておくことができるようにもなっている。
【0004】
この制御系から分かるように、一番外側のループである位置フィードバック系のエラー成分、すなわち位置指令から位置フィードバック値を差し引いた大きさ(位置の偏差)を監視すれば、衝突の有無を検出することができる。その最も基本的なものは、図15に示すように、位置制御ループにおける加算器4の直後からモータ1の位置をフィードバック制御した結果得られる位置偏差を取り出し、比較器5で「位置偏差のしきい値」である各関節における判定値6と比較して、大きければ衝突が発生したと判定するというものである。
【0005】
このような制御によれば、ショックセンサトーチがぶつかるとスイッチが入りサーボ電源が落ちる仕組みになっていたり、機械式の衝突検出機構が装備されている検出装置など従来から存在するものに比べれば、ロボットとして必要最小限の構成要素であるモータさえ備えることで、すなわち衝突センサを有しなくてもロボットコントローラに衝突検出ソフトを搭載するだけで衝突を検出することが可能となる。従って、ロボット本体の機構の複雑化をきたさないようにしておくことができる。
【0006】
衝突を検出する別の方法として、特許第2,749,724号に記載された外乱推定オブザーバを用いた衝突検出方法がある。これは、各サーボドライバの中に外乱トルクを観測するオブザーバを備え、実際にモータが回そうとしているイナーシャはどれくらいの大きさのものであるかを内部に持つモデルによって推定すると共に衝突により生じた外乱トルクを検出し、その外乱トルクの推定値に対する大小を見て衝突の有無を判定しようとするものである。
【0007】
すなわち、内部でモデル化することにより或る指令値に対してどのような挙動をするかを予測し、その予測挙動と実際の挙動とを突き合わせることによって先読み補償するという思想である。従って、イナーシャがモデル化できていればトルク予測は可能であり、イナーシャに変動がなければ高い精度で衝突を検出することができるはずである。複数の関節を備えたモデル化はともかくとして、単一の関節だけで扱うとすればイナーシャのモデル化は容易であるからである。
【0008】
このように、衝突の検出はぶつかったことによるサーボの状態変数の変化があって始めて可能となり、各モータ単位で検出されるトルクの変動によって衝突を感知しているのである。
【0009】
ところで、衝突を検出した後にロボットの動きを止める必要があるが、その停止のためにはいずれの関節においても可能なかぎりの大きさのトルクを発生させれば迅速に制動できるとの考えから、トルク制限器には最大トルク値が設定されるのが一般的である。最大トルクとはそれぞれのモータにおいて発生することができる最大のトルクを意味していることは言うまでもない。
【0010】
具体的には、位置フィードバックゲインを0とすることで速度制御系への指令を強制的に0とし、速度0を目標値とした速度フィードバック制御を行うことにより停止処理を行う。停止のための減速に使用するトルクは、速度制御系の後に配置されたトルク制限器により常にモータの最大許容トルクまで可能となっている。
【0011】
【発明が解決しようとする課題】
ところが、前記した図15の衝突検出システムにおいては、いずれかの関節の位置のエラー成分が十分に蓄積されなければ、衝突を検出したと認識することができない。それがために、衝突を検出した時点では既にモータが大きなトルクを発生した状態となっており、ロボット本体やツールの変形・損傷を可及的に少なくすることはできない。すなわち、衝突を検出することはできても、その時点で手遅れの状態になっているのである。
【0012】
例示すれば、図2に示した第3関節73 が第3リンクL3 を破線のごとく真っ直ぐな姿勢に保持しようとしていても、例えば第2関節72 によって押されれば障害物8に当たる。第3リンクL3 は障害物8によって実線のように動かされようとするがフィードバック制御が効いているので、第3関節L3 を駆動しているモータ13 が直ちに最大トルクで真っ直ぐな位置へ戻そうとする。当然に干渉力は増大し、大きな力が相互に作用することになってしまう。
【0013】
図15にも表されているように、サーボドライバー2にはトルク制御、その外側に速度制御、最外側に位置制御があるので、衝突に対して一番鋭敏なのはトルクとなる。その次に速度であり、一番鈍感なのが位置である。それゆえ、位置偏差で衝突を検出しようとすると、以下に述べる欠点は覆いようがない。
【0014】
フィードバック制御においては速度やトルクを発生させるために位置偏差が常に存在しており、それが高速でしかも大きな加速度で動作するとき大きくなることは言うまでもない。一方、ゆっくりと動作する場合には、障害物に当たったときでも位置偏差が小さい。
【0015】
そこで、小さな位置偏差で衝突であると判定させるようにしておくと、速い速度,大きい加速度で動作するときには小さな位置偏差を直ちに越えてしまうので衝突を誤検出することが多く、ロボットは止まってばかりで使い物にならなくなる。逆に、大きな位置偏差で衝突であると判定させるようにしておくと、遅い速度,小さな加速度で動くときには十分に行き過ぎなければ衝突を検出できなくなる。
【0016】
これから分かるように、通常考えられる一番厳しい動かし方をしても衝突と誤認識しないだけの大きな設定値を入れざるを得なくなり、位置偏差をもとにした衝突の検出は鈍感なものとなってしまう。
【0017】
特許第2,749,724号のように外乱推定オブザーバを使用した場合、そのオブザーバは一般的には速度ループに組み込まれる。これはサーボドライバにおいての話であるから各関節レベルで外乱を検出することになる。単一の関節だけで扱うとすればイナーシャのモデル化は容易であるが、複数の関節を持つ場合のモデル化は極めて難しく、多関節ロボットにおいては、衝突検出精度を十分に高く確保することが困難であると言える。
【0018】
複数の関節からなるロボットでは、その関節を駆動する任意のモータを同じ速度パターンで動作させても、モータが発生するトルクは、ロボットの姿勢によって異なるという特性を持つ。ロボットの関節位置すなわちロボットの姿勢や各関節の速度・加速度の状態により、モータが駆動すべき負荷イナーシャやモータが受ける外乱トルクが変動してしまうためである。
【0019】
言うまでもないが、ロボットは腕を伸ばせばイナーシャが大きくなり、短くすれば小さくなる。外乱推定オブザーバによる場合は高次数の制御となるゆえ、複数の関節でオブザーバを定義することが現実には不可能に近い。オブザーバの精度に要求される項目として、遠心力やコリオリ力等によっても生じる関節間の干渉が挙げられる。一つの関節でならこれらの速度の2乗項はたいした影響を持たないが、複数の関節からなる場合その影響は大きくなる。重力加速度による外乱トルクもロボットの姿勢が変われば変化する。回転半径が変わると重力によるモーメントも変わってくる。
【0020】
衝突の検出は、ぶつかったことによるサーボの状態変数の変化があって始めて可能となる。外乱推定オブザーバはサーボドライバに組み込まれるので外乱の検出はあくまでドライバレベルであり、従って衝突を感知するのは各モータ単位で検出されるトルクの変動であって、それは各関節レベルでの外乱検出にとどまることになる。
【0021】
実際問題としてモータの先端にどのような制御対象物が存在するか分かっていなければならないが、一つの関節レベルで外乱トルクを検出するといってもイナーシャ変動のない場合には正常に検出できても、イナーシャ変動のある場合やイナーシャ変動の避けられない多関節ロボットにおいては、そのトルク変動があったとき何処までが起こるべくして起こったものか、それとも衝突によって起こったものかは区別をつけがたい。すなわち、複数の関節があるとき上記した負荷イナーシャの変動に加えて、次に掲げる三つの外乱トルクが存在するからであり、その判定は難しい。
【0022】
複数の関節からなるロボットの各関節が受ける外乱トルクの種類は、以下の三つである。一つは、他の関節の加速度運動より受ける干渉トルク(ロボットの関節位置・加速度により変化する)であり、二つ目は、遠心力やコリオリ力による外乱トルク(ロボットの関節位置・速度により変化する)である。そして、三つ目は、重力加速度による外乱トルク(ロボットの関節位置により変化する)である。
【0023】
前述したごとく、トルクは衝突の検出において最も鋭敏であるが、これを検出の対象とする外乱推定オブザーバといえども、イナーシャ変動までは把握し得ない。なぜなら関節ごとに機能するからであり、他の関節を駆動するモータの状態と連動した上記の負荷イナーシャや外乱トルクの変動を考慮することができず、衝突によって発生した外乱トルクのみを精度よく抽出することは困難となる。このような事情を踏まえると、多関節ロボットの場合に厳密さを求めるにはまだ幾つかの課題が残されていると言わざるを得ない。
【0024】
ところで、前述したごとく、衝突後にロボットを止める際速度0を目標とした速度フィードバック制御を行うことにしているが、積分項が機能しているためにトルクは依然として発生し、それが大きくなることは避けられない。なぜなら、モータに最大トルクまで発揮させてもよいとしているのが一般的であり、モータが大きい力を発揮すれば、ロボットと障害物との間に大きな力が掛かることは必然的に起こるからである。すなわち、干渉があったときの逃げ動作が最大トルクで妨げられることになり、衝突による干渉力を全くと言ってよいほど軽減できなくなる問題がある。
【0025】
ちなみに、衝突発生時にトルク制限器を0に設定することはできない。ぶつかっているといえども慣性力があるので、慣性力に対して制動するのは摩擦だけということになり、制動力が著しく低下する。それだけでなく、停止している関節についても、衝突による衝撃力や他の関節から受ける干渉トルクによる関節の運動を制動できなくなるからである。
【0026】
本発明は上記した問題に鑑みなされたもので、その目的は、大きく分けて以下に記す二つの課題を克服する多関節ロボットにおける衝突検出・停止制御法を提供することである。第一の課題は、関節駆動トルクが非線形的に変動する複数の関節からなるロボットにおいても、衝突により発生する関節駆動トルクの変動から衝突が発生したことを高精度かつ短時間に検出することである。すなわち、衝突を感知しているのは各モータ単位で検出されるトルクの変動であるが、できるだけ厳密な計算をして衝突によるものか否かを見極めることができるようにしようとするのである。
【0027】
第二の課題は、ロボットの衝突検出後の停止処理において、衝突による障害物からの干渉力を和らげ、ロボット本体やぶつかった対象物の損傷を最小限に抑えることである。言い換えれば、大きな慣性力を持っていない関節もしくは速度が発生していない関節については、力方向に逃げる仕組みを提案しようとするものである。
【0028】
【課題を解決するための手段】
本発明は、複数の関節からなるロボットのリンクやリンク先端に把持されたツールまたはワークが周辺の障害物等に衝突したとき、リンク等の変形やロボット可動部の損傷を回避すべく衝突したことを検出しかつ直ちにロボットを停止させる方法に適用される。その特徴とするところは、図1を参照して、ロボットの各関節駆動用モータ11 ,12 ,13 を制御するためのサーボドライバ21 ,22 ,23 が検出したモータの位置θm・速度θd m・加速度θddmから、衝突が発生していない場合に各モータが発生すべきトルクT1 ,T2 ,T3 を、ロボットの各関節の位置θ、速度θd 、加速度θddとロボット機構パラメータとをもとにした
【数3】
で示す式に基づく逆動力学演算により予測する。そのトルク予測値と実際の関節駆動トルクTcとの差分ΔTを求める。差分ΔTの絶対値が予め設定された判定値εより大きい場合に衝突が発生したものと見做す。そして、複数の関節のうち停止もしくは低速で動作している関節あるいは慣性力が小さいため動作停止に必要なトルクが最大トルクと比較して十分小さい関節は、衝突があった後に衝突による干渉力を和らげるべく逃げ動作できるようにしたことである。
【0029】
ロボットの停止処理をするにあたり、衝突発生時の各関節の速度と負荷イナーシャからロボットの動作を停止させるために必要な最小限のトルク値を関節ごとに算出し、その最小限のトルク値をサーボドライバのトルク制限器3に設定すると共に全てのサーボドライバの位置フィードバック比例ゲインKppを0とし、強制的に速度指令値を0とした速度フィードバック制御による減速処理を行わせるようにする。
【0030】
ロボットの動作を停止させるために必要な各関節のトルク値の算出にあたっては、全ての関節が同じ時間で停止すると仮定して、関節ごとに減速停止時間tmd1 ,tmd2 ,tmd3 を算出したうえで、全ての減速停止時間のうちの最大値をいずれの関節の減速停止時間にも充てるようにすることが好ましい。
【0031】
衝突が発生していない場合に各関節を駆動するモータが発生すべきトルクを予測するとき使用されるモータの位置・速度・加速度として、図9に示すように、サーボドライバ21 ,22 ,23 に位置指令すべく予め教示されている作業データの再生動作により演算した関節位置θと、その関節位置をもとにして計算された速度θd および加速度θddを採用してもよい。
【0032】
逆動力学演算においては、ロボットの関節数をnとすれば、ロボットの各関節の加速度により各関節で発生するトルクならびに他の関節の加速度により発生する干渉トルクを示すn×nの慣性行列、遠心力とコリオリ力の影響を示すn×1の粘性行列、および重力加速度の影響により発生するトルクを示すn×3の重力行列が含まれる。
【0033】
n×nの慣性行列における対角要素としての「関節の持つ加速度が該関節自体に及ぼすトルクの関係を示した慣性モーメント」には、減速機を含むモータロータの慣性モーメントが減速比の2乗倍して加算され、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮できるようにしている。
【0034】
逆動力学演算に供される式に、
【数4】
ただし、Cmi 関節iにおける減速機の動粘性係数
(i=1,2,・・・n 以下同じ)
θdi 関節iにおけるモータの速度応答
Fcou i 関節iにおける減速機のクーロン摩擦トルク
sgn (θdi ) 符号関数
= 1 ; θd m>0のとき
= 0 ; θd m=0のとき
=−1 ; θd m<0のとき
を加算しておき、各関節内のモータ軸受や減速機の摩擦抵抗を考慮することができるようにしておくとよい。
【0035】
ロボットの機構パラメータには、各関節に連結されるリンクの重心位置・質量・長さが含まれていればよい。さらに、慣性テンソルも付加されているならば、より一層好ましい。
【0036】
トルク予測値との差分を求めるときの実際の関節駆動トルクには、実際にサーボドライバが生成したトルク指令値から算出されるトルクを充てておけばよい。
【0037】
各モータにおけるトルク応答値から算出されるトルクを採用すれば、衝突検出精度はより一層高められる。
【0038】
【発明の実施の形態】
以下に、本発明に係る多関節ロボットにおける衝突検出・停止制御法を、その実施の形態を表した図面を参照しながら詳細に説明する。まず、本発明が適用される例としてのアーク溶接ロボットの構成の概略、ならびにリンクを連結している関節を駆動するモータの制御から述べる。なお、本発明は、ロボットが作業中にワーク等の外界の対象物やその他の障害物に衝突したとき、いち早くかつ精度よく衝突のあったことを検出し、以後速やかにロボットの動きを止めたりリンクに作用する力の全部もしくは一部を解放して、ロボット本体やツールもしくはワークの変形や損傷等を軽減または回避できるようにしようとするものである。
【0039】
詳細は図や表をもとにして後述するが、複数の関節からなるロボットにおけるいずれかのリンクの逃げ動作を概略的に示す図2を参照して、本発明の基盤となるところを説明する。この例は三つの関節71 ,72 ,73 からなるロボット10であり、第1関節71 および第2関節72 の高速動作中に第3リンクL3 が障害物8に衝突した場合を表している。述べるまでもないが、図中の矢印9のように、停止しているか動きの少ない第3関節73 が衝突による干渉力を和らげる方向に回転すれば、ロボット全体としての衝突による衝撃をおおいに緩和することができる。
【0040】
ところで、従来技術の項でも触れたが、関節駆動トルクが非線形的に変動する複数の関節からなるロボットにおいては、衝突が発生していないときでも以下の要因を考慮して、各関節が発生するトルク値を正確に予測しておくことが重要である。それらをもとにして、衝突が原因で発生する関節駆動トルクの変動を検出すれば、衝突の発生したことを高精度かつ短時間に検出することができるようになるからである。
【0041】
その要因とは、(1) ロボット各関節の位置の変化により生じる負荷イナーシャの変動(「イナーシャ変動」という)、(2) ロボット各関節の位置・加速度の変化により生じる他の関節の加速度運動から受ける干渉トルクの変動(「加速度の干渉」という)、(3) ロボット各関節の位置・速度の変化により生じる遠心力やコリオリ力からなる外乱トルクの変動(「速度の干渉」という)、(4) ロボット各関節の位置変化により生じる重力加速度の影響による外乱トルクの変動(「重力の干渉」という)の四つである。
【0042】
本発明の最初の例を説明する制御ブロック線図は、図1に表されている。複数の関節を持つ図2に示したロボット10のリンクL1 ,L2 ,L3 やリンク先端に把持されたツールまたはワーク(図示せず)が、周囲の障害物8に衝突したときリンクやツールの変形を防止しさらには関節等のロボット可動部の損傷を回避すべく、図1中の衝突検出部11で衝突したことを検出し、かつ直ちに停止処理部12においてロボットを停止させることができるようになっている。なお、3つの関節それぞれのためにモータ11 ,12 ,13 が装備され、そのモータごとにロボットコントローラ13からの指令を受けて動作するサーボドライバ21 ,22 ,23 が準備されている。
【0043】
本発明に係る制御における衝突検出処理の主たるところは、以下の工程からなる。まず、ロボットの関節駆動用の各モータを制御するサーボドライバ21 ,22 ,23 が検出したモータの位置θm1 ,θm2 ,θm3 、速度θd 1 ,θd 2 ,θd 3 、加速度θdd1 ,θdd2 ,θdd3 から、衝突が発生していない場合に各モータが発生しているはずのトルクT1 ,T2 ,T3 を、後述する逆動力学演算によって予測する。
【0044】
次に、そのトルク予測値T1 ,T2 ,T3 と、実際の関節駆動トルク、すなわちサーボドライバ21 ,22 ,23 が実際に生成したトルク指令値Tm1 ,Tm2 ,Tm3 から算出されるトルクとの差分ΔT1 ,ΔT2 ,ΔT3 を求める。そして、いずれかの差分ΔTi の絶対値が予め設定された該当する判定値εi より大きければ、衝突が発生したものと見做すようにしている。
【0045】
衝突の検出は、ぶつかったことによるサーボの状態変数の変化があって始めて可能となることは、既に述べた。また、衝突を感知しているのは各モータ単位で検出されるトルクの変動であることも触れた。そのトルクの変動が起こるべくして起こったものか衝突によって起こったものかは、複数の関節があるとき上で述べた四つの外乱トルクの存在を見極めなければ判断することができない。しかしながら、その存在を見極めるためには、厳密な計算が求められることになる。
【0046】
そこで、ロボットが所望の運動をするために必要な各関節に発生させるべきトルクを求め、これを実際にモータが発生しているトルクと突き合わせ、その違いから衝突の有無を検出することにする。演算量が多くなる傾向にはあるが、検出はより一層正確なものとなる。この例では、モータが励磁された後は何時でもサーボドライバからモータの現在位置・速度・加速度を取り込み、それらで各関節が動いているとして各モータに出ているはずのトルク値を計算するというものである。
【0047】
ここで、まず図3を参照して、ロボットシステムの構成について説明する。ロボットは用途に応じて適宜の数の関節を備えるが、図の簡潔化を図るために既に示したとおり、3つの関節からなるロボットを例にする。ロボットの各関節を駆動するモータ11 ,12 ,13 のそれぞれには、回転による前後進量としての位置情報を提供するエンコーダ1e1 ,1e2 ,1e3 が取り付けられている。
【0048】
これらのモータを駆動するための制御装置(ロボットコントローラ)13はロボット本体とは別の制御ボックスに配備され、ティーチペンダント14で指令された教示データを記憶し、その記憶データと予め設定記憶されている当該ロボット固有の機構パラメータ等とをもとにして動作を再生させる演算が行われるようになっている。
【0049】
そして、コントローラ13とモータ1との間には、各モータを位置・速度制御するサーボドライバ21 ,22 ,23 や、位置・速度制御により求められた指令電流値をもとにしてモータに電力を供給するサーボアンプ2a1 ,2a2 ,2a3 も設けられる。ちなみに、サーボドライバ2の構成は従来の技術の項で述べた図14のとおりであり、これが図1の下半部に記載しているかたちで搭載されている。
【0050】
各サーボドライバには、制御装置から与えられる位置指令にモータを追従制御させるため、位置制御ループと速度制御ループとが組み込まれている。位置制御ループは比例要素によるフィードバック制御が、速度制御ループについては比例要素および積分要素によるフィードバックが行われる。なお、位置フィードバック比例ゲインKppは、ロボットコントローラからの要求に応じて、任意の値に変更することができる。
【0051】
速度制御系の後にはトルク制限器3が配置され、速度制御により生成されるトルク指令値Tmi を任意の値で制限しておくことができるようになっている。トルク制限器3に設定される制限値も、コントローラ13からの要求に応じて任意の値に変更可能である。なお、図中のKvpは速度フィードバック比例ゲイン、Kviは速度フィードバック積分ゲイン、Sは微分要素、1/Sは積分要素を表している。
【0052】
コントローラ13は3つの関節に対する制御指令を発するもので、大別すると図3に表したように、中央処理装置CPU,演算プログラムや各種データを記憶して演算する記憶演算部、教示データの取り込みや演算値の出力をするためのインターフェイスといったものからなる。ここで、教示されたデータ(以下作業データという)の再生動作を、以下に簡単に述べる。
【0053】
制御プログラムはROMに格納されており、その中の軌道計画部22が図1に示すようにハードディスク15に格納されている作業データを読み出し、ロボット先端に把持されるツールまたはワークの作業軌道が三次元空間の直交座標上で計画される。
【0054】
次いで、補間点計算部16がその作業軌道を予め規定された時間の補間周期ごとに分割し、補間周期ごとにツール等が到達すべき位置・姿勢を補間点データとして計算する。そして、逆変換部17が補間点データ中の直交座標で表された位置・姿勢を、多関節ロボットで再現するに必要な各関節レベルでの位置に逆変換し、各サーボドライバ21 ,22 ,23 への位置指令値を演算する。
【0055】
このサーボドライバへの位置指令はRAMに一時的に記憶され、ロボットの関節位置をサーボ位置指令値にデータ変換するデータ変換部18から、同期信号SYNC に同期して各サーボドライバへ伝達される。なお、以上の処理は、多関節ロボットをティーチングプレイバック方式で制御するうえで最も基本的な処理であり、公知であることは言うまでもない。
【0056】
ちなみに、図3中のROMには、ロボットの動作制御ならびに衝突検出や停止処理を実行するプログラムと、その実行条件や制御定数などのデータが格納される。RAMはCPUのワーキングエリアとして用いられ、計算途中のデータを一時的に保存する。ハードディスク15には作業データのみならず、ロボット固有のデータ例えばリンクの長さ・質量といったものや、教示された動作において固有のものであるワークやツールに関するデータ等が格納される。
【0057】
シリアルIO(インプット・アウトプット)19はティーチペンダント14と接続され、ティーチペンダントの操作による作業データがシリアル通信によってCPUで認識できるようになっている。サーボIF(インターフェイス)20はサーボドライバ21 ,22 ,23 に接続され、指令データの送信やモータの制御状態のモニタを可能にする。タイマ21は、定期時刻ごとに同期信号SYNC を発生し、この信号によってサーボドライバへの指令値の更新タイミングが与えられる。
【0058】
次に、本発明によるロボットの衝突検出ならびに衝突後の停止処理を行うための最初の形態例を、図1に基づいて説明する。なお、停止処理は後述することにして、衝突検出から述べる。衝突の検出における基本的なところは、比較器291 ,292 ,293 における比較の対象を、位置偏差ではなくトルクとしている点である。
【0059】
ところで、トルクをもとにして衝突の有無を検出しようとする場合、大きいトルクが掛かっていても一概に衝突しているとは言えないことに留意しなければならない。関節が大きな加速度で動くときは大きなトルクが要求されるので、トルクが急激に大きくなったからといって衝突と見做すわけにはいかない。よって、本来所望する運動をさせるために必要なトルクは幾らか、今出ているトルクとはどの程度かけ離れているかを厳密に判定しなければ、高精度な衝突検出はできなくなる。本発明はこのような観点から研究を重ね、完成したものである。
【0060】
まず、サーボドライバはモータの駆動を制御するだけでなく、サーボモータの状態をモニタできるものであるから、本例においてはサーボドライバからモータの位置・速度・加速度といったモータ情報を取り出すことにする。そして、これをコントローラ13の衝突検出部11に入力して、後で詳しく述べるH(θ)・θdd+C(θ・θd )+G(θ)・gなる計算式をもとにした逆動力学演算を行い、衝突していなければ各関節駆動用のモータが発生するはずのトルクT1 ,T2 ,T3 を予測する。
【0061】
逆動力学演算とは、ロボットの各関節の位置・速度・加速度の状態と、ロボットの機構パラメータから、ロボットの各関節に発生するトルクを算出するものである。この逆動力学演算によれば、前記した(1) から(4) のトルク変動要因の影響を正確に予測することができるようになる。
【0062】
一方、予測トルクと比較される対象の「実際に発生しているトルク」は、サーボドライバのトルクループの手前で拾い上げられたトルク値である。衝突がなければ、逆動力学で演算されたトルク値とほとんど同じであり、両トルクの差が大きければ衝突していることを意味することになる。従って、その差分が関節ごとに予め決められた後述する「しきい値」εi に達していれば、「衝突した」と見做すことにするのである。
【0063】
i =H(θ)・θdd+C(θ・θd )+G(θ)・gを計算するにあたり、そのロボット固有のデータすなわち機構パラメータも必要であり、それらがハードディスクから読み出される。トルク値は電流値でもN・m(ニュートン・メータ)でもどちらでもよい。サーボドライバの中にトルク定数(N・m/A)を持ったトルク換算部があればN・mで出力することができ、それを演算値と突き合わせればよい。サーボドライバにトルク換算部がなければ、逆動力学演算において電流値に換算して比較すればよいことは言うまでもない。
【0064】
各サーボドライバとロボットコントローラとの間にはサーボIF20が設置され、以下のデータが伝達される。図1に示されているように、サーボドライバ2からコントローラ13へは、モータの位置応答θm[rad]、モータの速度応答θd m[rad/sec]、モータの加速度応答θddm[rad/sec2 ]およびモータへのトルク指令値Tm[N・m]であり、コントローラからサーボドライバへは、位置指令値θ[rad]、位置フィードバック比例ゲインKppおよび後述するトルク制限値Tmdec [N・m]である。
【0065】
衝突の検出は、サーボドライバのフィードバック制御が行われている限り、図4に示す5つのステップが順次繰り返される。まず、ステップ1(図面では以下S1などと記す)では、衝突検出部11がサーボIF20を介してサーボドライバ2から、ロボットの各関節を駆動するモータの位置・速度・加速度の応答値とトルク指令値を読み込む。この読み込みは、関節を駆動する全てのモータに対して行われる。3つの関節からなるロボットの場合では、モータの位置・速度・加速度の応答値ならびにトルク指令値は、表1の記号を用いて、それぞれ式(1-1) ないし(1-4) で表される行列となる。なお、式中の記号の添え数字は関節番号を表し、[ ]T は表1の「個数」の欄に記載された行列を「転置」して表示したこと示している。
【表1】
【数5】
【0066】
ちなみに、実施例では関節数を3としているが、本発明の趣旨からしてn≧2で適用することができる。n=1の場合も理屈上は不可能でないが、低自由度のロボットは通常速度は速くなくまた他関節からの干渉も受けないので、行列演算の必要性はない。なお、例えば一方にはツールが装着され他方にワークが把持された2基のロボットが連動して再生動作するシステムの場合であっても、本発明の思想は各ロボットごとに適用されるものである。
【0067】
ところで、モータとロボット関節とは、減速機を介して結合されるのが一般的である。そのため、式(1-1) ないし(1-4) の値を、ロボット関節での位置・速度・加速度に換算する必要がある。関節における位置・速度・加速度の応答値ならびに駆動トルクは、表2の記号を用いてそれぞれ式(1-5) ないし(1-8)で表すことができる。
【表2】
【数6】
【0068】
第i関節を例にすれば、それらは式(1-10)ないし(1-13)で表される。位置・速度・加速度については減速比ζi で除した値が関節換算値となり、トルクにおいては減速比を乗じた値が関節換算値となる。そこで、ステップ2においては、モータレベルの位置・速度・加速度がロボットの関節レベルの値に変更される。
【数7】
【0069】
関節レベルの位置・速度・加速度ならびにトルクの応答値が分かれば、ステップ3においては、これらを次に述べる逆動力学モデル(H,C,G)に適用した逆動力学演算によって、発生すると思われるトルクが計算される。逆動力学モデルには、慣性行列のH行列と粘性行列のC行列と重力行列のG行列とが含まれている。なお、各行列の物理的意味は後述する。
【0070】
ロボットの機構パラメータ(関節に連結されるリンクの重心位置・質量・慣性テンソル・長さ等)と、各関節の位置・速度・加速度から、各関節に発生するトルクTは式(2-1) により演算できることが知られている。これを逆動力学モデルという。
【数8】
【0071】
式(2-1) は、広瀬茂男著「ロボット工学−機械システムのベクトル解析−」裳華房、第2版・1989年2月10日発行」(以下技術文献1という)に記載されている。その節11.3の〔4〕には、剛体の運動解析手法の一つであるニュートン・オイラー法を用いて逆動力学演算する手順が詳しく説明されているので、ここでは説明を省く。最終的に逆動力学演算式は技術文献1中の式(11.28) となるが、その式中の最後の項JT K(転置したヤコビ行列にロボットの先端に掛かる外力と外力によるモーメントを表すKを乗じたもの)は、本発明の場合、ロボットが衝突していない状態、すなわち空間を何物にも触れずに変位しているときのトルクを求めようとすることから考慮する必要がなく、従ってその項を省くことによって上記の式(2-1) を得ることができる。
【0072】
関節数nのロボット場合、慣性行列Hはn×nの正則な対称行列であり、ロボットの各関節の加速度によって関節自体に発生するトルクならびに他関節の加速度によって発生する干渉トルクを意味する。粘性行列Cはn×1行列であり、ロボットの各関節の速度により生じる遠心力およびコリオリ力の影響で生じるトルクを示し、重力行列Gはn×3行列で、重力加速度の影響により発生するトルクを示すものである。
【0073】
慣性行列Hについて、少し詳しく触れる。慣性行列H(後述する式(2-9) を参照)の対角要素のHjjは、第j関節の加速度が第j関節自体に及ぼすトルクの関係を示す慣性モーメントを表しており、すなわちHjjは関節位置により変動する負荷イナーシャとなる。その他の成分であるHijは、第j関節の加速度が第i関節に及ぼす干渉トルクの関係を表す慣性モーメントである。
【0074】
慣性行列H,粘性行列Cおよび重力行列Gは、ロボットの機構パラメータと各関節の位置・速度から計算できる。ロボットの機構パラメータは、以下の4つから構成される。なお、表3中の個数は、関節に連結されるリンク1つ当たりである。ちなみに、図2に表したロボットの場合の第3リンクL3 に溶接トーチ等のツールが装着されているならば、その第3リンクのパラメータはツールが一体となった状態で与えられることになる。
【表3】
【0075】
各リンクの質量・重心位置・慣性テンソル・長さ等のロボット機構パラメータは、ロボットの機構やリンク形状および材質から予め求めておくことが可能な既知の値であり、それがロボットの運動中に変化しないことも明らかであるから固定値であって、予めハードディスク15の中に記憶される。それらの機構パラメータを読み出して式(2-1) に適用すれば、各関節の現在の位置・速度・加速度の3つと重力加速度gとから、各関節に発生するトルクを演算することができる。なお、式(2-1) の各要素は、表4に記載の意味を持つ。
【表4】
【0076】
ここで、本例におけるロボットの機構パラメータについて説明を加える。質量mi 、重心位置mxi ,myi ,mzi 、慣性テンソルIxi ないしIzi は、第iの関節の動作により移動しかつ第i+1の関節の動作によっては移動しない第iの関節に連結されるリンクの質量、同じくリンクの重心位置、同じくリンクの慣性テンソルをそれぞれ示す。なお、慣性テンソルの演算手順は、技術文献1の節3.8に詳しく説明されているので(式(3.38)を参照)、ここでは説明を割愛する。
【0077】
上記した重心位置および慣性テンソルを演算するうえでの基準は、リンク座標系に置かれる。関節ごとに割り当てるリンク座標系の定義手順が、吉川恒夫訳「ロボット・マニピュレータ」コロナ社、初版・1989年12月15日発行(以下技術文献2という)の節2.9に詳細に説明されているが、ここでは、基本的なリンク座標系の定義を図5において説明する。例えば、第2関節72 と第3関節73 に跨がるリンクL2 においては、第2関節の回転中心を原点として第2関節に割り当られるリンク座標が定められ、そのx方向をリンクL2 の長さ方向とし、z方向を第2関節の回転方向、y方向を右手座標系の方向と決める。
【0078】
次に、慣性テンソルIは一般に物体の回転しにくさを示すために使用され、よく知られているとおりの3×3の対称行列である式(2-2) で表される。その対角成分はロボット機構部の各関節の慣性モーメントを、その他の成分は慣性乗積の負の値を表すものとなっている。
【数9】
【0079】
また、慣性モーメントは次式で求められ、物体(ロボットリンク)の回りにくさおよび止めにくさを表す。
【数10】
【0080】
次いで、慣性乗積は次式より求められ、物体(ロボットリンク)が回転運動を行っているとき、その運動を乱すモーメントを表す。
【数11】
ここで、dmは微小体積の質量を示す。また、式中のx,y,zは、所定のリンク座標系を基準として表され、図5に示すリンクL2 の慣性テンソルの基準座標は、第2関節に割り当てられたリンク座標系をリンクL2 の重心位置に移したときの座標系とする。他の関節についても同様に定義する。
【0081】
なお、慣性,粘性および重力の行列値の演算方法は技術文献1に詳しく説明されている。ここではその説明は省くが、3つの関節からなるロボットの場合、慣性,粘性および重力の行列は次式のように表される。
【数12】
【0082】
式(2-9) ないし式(2-11)の各構成要素は表3に示されたパラメータならびに関節位置θ,関節速度θd のみから構成され、θ,θd 以外はすべて既知の値である。従って、ステップ2で求められる関節位置応答θr,関節速度応答θd rを次式のごとくθ,θd に置けば、式(2-1) 中の慣性行列H,粘性行列Cおよび重力行列Gを求めることができる。
【数13】
【0083】
ステップ3でH,C,Gの各行列が算出されたことで、式(2-1) 中の未知の値は関節加速度θddならびに重力加速度が働く方向を示す重力加速度ベクトルgの二つとなる。後者はロボットの設置姿勢によって変わるものであるが、一般的にはロボットの設置姿勢が変わらないと考えて差し支えなく、従って予め規定しておくことができる。
【0084】
ステップ4において、最終的に式(2-1) で表される逆動力学モデルを計算するうえで必要な値のうち未知なものは、関節加速度θddだけとなる。これもステップ2で求められた関節加速度応答θddrを式(4-1) のかたちで代入すれば、衝突のない場合にロボットの各関節がθr,θd r,θddrで運動するために必要となる各関節での発生トルクTを、式(4-2) で予測することができる。
【数14】
【0085】
ステップ4で算出した衝突が起こっていない場合にロボットの各関節がθr,θd r,θddrで運動するために必要な各関節のトルクTと、ステップ2で算出した実際にモータが関節を駆動しようとしているトルク指令値Tcとの差分ΔTをステップ5において計算する。
【数15】
【0086】
衝突がなければ、トルク指令値Tcと予測値Tとはほとんど等しく、ΔT≒0となるはずである。一方、ΔTの絶対値が予め設定された判定値εi よりも大きくなる場合には衝突が発生したものと見做して、制御プログラムを停止処理へと移行させる。具体的には、式(5-3) ないし式(5-5) の条件式のいずれか一つでも満足する状態が生じれば、衝突が発生したと判定される。
【数16】
【表5】
【0087】
ちなみに、εi は例えば0.1Tci もあれば十分であることが多い。現実には0.3Tci なければならない場合もあるが、そのときでも3割増し以上のトルクが掛かれば衝突と判定することになるわけで、従来技術の項で述べた位置偏差による場合、トルクに換算すれば例えば0.8Tci もしくはそれ以上にも及ぶのに比べると、本発明の場合衝突検出精度が格段に向上することが分かる。このような向上は、関節によってはさらなる改善も期待できる。
【0088】
なお、判定値εi は固定しておくといった単純な扱いをしてもよいが、加速度の大きさや速度の大きさに応じて可変とし、加速度や速度が大きければ感度を下げ、小さければ上げるといったように感度変更可能としておくこともできる。加減速時に感度を上げて0.1Tcといったようにしておくと衝突の誤検出を招きやすく、加減速が少なくかつ低速のときに0.3Tcといったように感度を下げておくと衝突の検出が遅れることになるからである。
【0089】
以上の説明から分かるように、ロボットの衝突の検出のためには、全ての関節を駆動する各モータの情報を統括して処理し、それらを統合して逆動力学演算することにより全ての干渉(衝突)を見い出すことができるようにしている。その演算において、式(2-1) が不可欠であることは言うまでもない。ロボットの機構パラメータのうちリンク質量・リンク重心位置・リンク長さも欠くことのできない要素である。しかし、リンク慣性テンソルは細長いリンクを採用するロボットでは必要となるが、軽量ロボットまたは短リンクロボットなどの場合に考慮するに及ばないことがあるので、その都度適宜選択することにすればよい。
【0090】
サーボドライバ21 ,22 ,23 に入力される「位置指令」はハードディスク15に記憶されたデータに基づき軌道計画部22で演算された結果として出された命令である(図1を参照)が、教示中にあってはティーチペンダント14(図3を参照)によって入力されたデータに基づく「位置指令」ということになる。それゆえ、教示中の不意の衝突に対しても、本発明の動作を行わせることができる。
【0091】
本発明に係る衝突検出法によれば、衝突検出が精度高く迅速になされるので、後述する停止処理操作とあいまって、少なくともロボットが壊れたり周辺に存在する治具等の機械的構成要素を損傷させたり、復旧に多大の時間を要することは可及的に少なくなり、衝突後の稼働や教示を速やかに再開することができるようになる。衝突検出はコントローラに搭載した衝突検出ソフトによるので、ショックセンサや機械式衝突検出装置をロボットリンクに装備する必要はなく、ロボットリンクの構造や機構の複雑化は回避され、また軽量化も図られる。
【0092】
ところで、図1においては、サーボドライバからコントローラに、モータの位置応答,モータの速度応答,モータの加速度応答およびモータへのトルク指令値が伝達されると説明した。しかし、速度応答と加速度応答とはそれぞれの前者を微分すれば得られるものであるので、サーボドライバからコントローラへは、図6のようにモータの位置応答と速度応答にとどめ、衝突検出部11において速度応答値を微分してモータの加速度応答を得るようにしてもよい。さらには、モータの位置応答のみをコントローラへ供給し、図示しないが、コントローラで一階および二階微分して速度および加速度を求めるようにすることも可能となる。いずれの場合も、サーボIF20を介した情報伝達の混雑を緩和させることが期待できる。
【0093】
上記した例では、差分ΔTを求めるにあたり、逆動力学演算に基づくトルク予測値と、サーボドライバが速度制御により生成したトルク指令値Tmを実際の関節駆動トルクと扱い、そのトルク指令値を採用すると説明した。しかし、そのトルク指令値に代えて、各モータにおけるトルク応答値から算出されるトルクを採用してもよい。
【0094】
図7から分かるように、サーボドライバ21 ,22 ,23 で求められたトルク指令値Tmi に応じた電力を動力線から各ブラシ付きDCモータ11 ,12 ,13 に供給するサーボAMP2a1 ,2a2 ,2a3 からの回路に電流検出器231 ,232 ,233 を設ける。そして、電流検出器で拾った電流をA/D変換した後にそのトルク値Tni にトルク定数を乗じて、N・mとして得たトルク応答値(実際に出力されているトルク)Tci ’を使用する。この場合、前記した式(1-13)の代わりに次式が採用されることになる。
【数17】
【0095】
モータがブラシレスDCモータである場合には、実際に流れている全ての電流が有効なトルクになるわけではない。特にモータの高速回転時には、トルクに変換されない無効電流が増加し、指令と出力の誤差が大きくなる。実際にモータが出力しているトルクは、モータの三つ相のうち二つの相に流れている電流(以下相電流という)を検出して、図8に示す座標変換部24で座標変換を行うことにより、トルクに変換される電流である有効電流iqaを求めることができる。
【0096】
座標変換処理は無効電流を捨て有効分だけ取り出すというものであり、これについては、杉本英彦他著「ACサーボシステムの理論と設計の実際」総合電子出版社、第2版・1990年5月8日発行(以下技術文献3という)の節4.2で詳細に説明されている。最終的には当文献中の式(4.11)で表される。式中のiqaの項が有効電流に相当するもので、相電流iua,ivaとモータの電気角θreより有効電流iqaが式(6-1) で算出される。
【数18】
【0097】
なお、電気角θreの検出には多くの方法があるが、絶対値エンコーダを使用する場合は次のようにして求めることができる。まず、電気角360°に相当するエンコーダのパルス変位θe rev を算出する。θe rev は、エンコーダ1回転当たりのパルス変位θmrev をモータの極対数Pn で除した値であり、式(6-2) で算出される。極対数とはブラシレスDCモータのu,v,wの三つの極対がモータ1回転当たりに配置されている数であって、一つの極対に相当するモータの回転量が電気角360°となる。エンコーダ1回転当たりのパルス変位θmrev 、モータの極対数Pn についても、それぞれエンコーダとモータの仕様から予め規定しておくことができる値である。
【数19】
【0098】
エンコーダからの出力パルスθeを電気角360°に相当するエンコーダのパルス変位θerev で除し、そのときの余りをθerev で除したものに2πを乗じることによって、電気角θreが求められる。具体的には、式(6-3) となる。式中の演算子MODは、除算の余りを算出することを表している。
【数20】
【0099】
以上の式(6-1) ないし式(6-3) より算出された有効電流iqaにトルク定数Kt を掛けることにより、モータが実際に出力しているトルクTnが算出される。トルク定数Kt は有効トルクに対する発生トルクTnの比で、モータの仕様から予め規定しておくことができる値である。
【数21】
【0100】
上に現れた記号を参考までに、表6に掲記しておく。なお、上記した図8は出力トルクの検出機能を持つサーボドライバのブロック線図であり、電流検出器23u ,23v で検出された相電流iua,ivaが、AD変換器を介して読みとられている。電気角の算出のため必要となるエンコーダの出力パルスも、読みとり可能となっている。
【表6】
【0101】
なお、モータがブラシ付きDCモータの場合には、図8中の座標変換は必要なく、直ちにトルク定数を乗じればよい。このようにモータの出力トルクを検出して、これを衝突検出に供するようにすれば、より一層検出精度が向上する。とりわけ、高速回転時にトルクに変換されない無効電流が増加するためにトルク指令値とモータ出力トルクとの間にずれの生じやすいブラシレスDCモータ等においては、極めて有効な手段となる。
【0102】
図1では、サーボドライバから拾い上げた位置・速度・加速度応答値を用いて逆動力学演算するようにしているが、拾い上げてから演算に入る関係上、後述する停止処理に入るまでに若干の遅れが出る。図9はそれを改善しようとするもので、衝突が発生していない場合に各関節を駆動するモータが発生すべきトルクを予測するときに使用されるモータの位置・速度・加速度として、作業データの再生動作のために演算した関節位置とその関節位置をもとにして計算した速度および加速度を採用するものである。
【0103】
モータがいかなる位置・速度・加速度で動いているかは軌道計画の段階で分かるはずであり、これを使用すればサーボの状態を見なくても予測トルクを計算しておくことができるとの知見に基づくものである。従って、軌道計画を解析した結果得られる位置・速度・加速度から各モータで発生すると思われるトルクを逆動力学演算によって求め、それを実際にモータに流れているトルクと突き合わせて衝突の有無を検出しようとするのである。
【0104】
そのために、図9のコントローラ13内の制御プログラムには、図1に示した軌道計画部22,補間点計算部16,逆変換部17に加えて、補間周期ごとのロボットの関節位置からロボット関節の速度・加速度を計算する速度・加速度計算部25が付加されている。
【0105】
なお、データ変換部18からサーボドライバ2への位置指令信号の出力と逆動力学演算のためのデータ信号26の出力との間に時間的調整をとっておく必要があるが、その場合、後者を前者より例えばサーボドライバでの演算時間だけ遅らせるといった配慮をすればよい。この図9のような構成にすれば、サーボドライバ2とコントローラ13との間のデータのやり取り量も少なくなり、通信の負担が軽減されることにもなる。ちなみに、ティーチペンダントによる教示中や手動運転中でも、軌道計画部22で作業軌道を生成して関節の位置・速度・加速度を予測することが可能であるので、この方法は先の例(図1を参照)と同様に、動作再生時以外でも適用することができる。
【0106】
衝突の検出は、ロボットの作業軌道の補間点データに対してサーボドライバ2のフィードバック制御が行われている限り、図10に示す6つのステップを順次繰り返して行うことにより実現される。なお、衝突検出は、コントローラ13内のプログラムである衝突検出部11で行われることは、図1の例と異なるところがない。
【0107】
まず、ステップ11においては、ロボット各関節の位置・速度・加速度が、ハードディスク15に記憶される作業データの再生動作行う過程で計算される。具体的には、速度・加速度計算部25で補間周期ごとのロボットの各関節位置θからロボット各関節の速度θd ,加速度θddが計算される。
【0108】
ここで、ロボット各関節の速度・加速度を計算する方法について説明する。補間周期Smpt ごとのロボット各関節の位置θが算出されると、現周期で算出された関節位置をθcurrとし、直前の補間における関節位置をθprevとして、現補間点おける各関節の速度θd が以下のようにして算出される。
【数22】
【0109】
現周期で算出された関節速度をθd currとし、直前の補間時の関節速度をθd prevとすれば、現補間点おける各関節の加速度θddが同様の要領で求められる。なお、時刻0における、各関節の速度,加速度はいずれも0と置かれる。
【数23】
【0110】
ステップ11で算出したロボット各関節の位置θから、サーボドライバへの位置指令θc を式(8-1) により算出し、サーボIF20を介してサーボドライバ2へ出力する。モータは減速機を介して関節を駆動するので、ロボット各関節の位置θに減速比を乗じた値がサーボドライバへの指令値となる。このようにして、サーボドライバへの位置指令値の送信がステップ12で行われる。なお、θc はロボット関節の位置応答 [rad] であり、その個数はn(=3)×1行列となる。
【数24】
【0111】
ステップ13ではトルクの状態の読み込みがなされる。すなわち、衝突検出部11がサーボIFを介してサーボドライバ2から、ロボットの各関節を駆動するモータのトルク指令値Tm1 ,Tm2 ,Tm3 を読み込む。また、モータからロボット関節への結合は減速機を介して行われるので、サーボドライバ内のトルク指令値Tmi に減速比を乗じた値が関節を駆動するトルクTc i として与えられる。
【数25】
【0112】
以後、ステップ14では逆動力学モデル(H,C,G)が計算され、ステップ15で逆動力学演算によるトルク予測値が演算される。ステップ16で前述したとおりの手順によって衝突判定処理がなされる。もちろん、その後には後述する停止処理も行われる。
【0113】
図9の例においては位置・速度・加速度指令をもとにしてトルクを予測しており、実際の応答値と指令値の誤差が小さければトルク予測の精度が上がることは明らかである。従って、衝突検出精度も向上することになる。そこで、図9の構成に、サーボドライバ2内のサーボ制御系に対して速度指令をフィードフォワード補償すれば、サーボドライバの指令追従性能を上げることができ、常にロボットの衝突検出精度を高く維持しておくことができるようになる。図11は、図9の構成に速度指令のフィードフォワード補償を付加した形態を示している。
【0114】
もう少し詳しく述べると、図9の場合、軌道計画の後に関節速度も演算されているので、図11のように速度をデータ変換部27からフィードフォワードすることができる。フィードフォワードすれば、位置・速度・加速度の関係が実際の応答と異なる程度を図9の場合よりも小さくすることができる。すなわち、図9の場合、位置・速度・加速度と実際に使用される位置・速度・加速度とが乖離する傾向にあり、従って衝突していないにもかかわらず逆動力学演算結果とトルク指令値とが一致しなくなるといった演算精度上の問題が生じる。しかし、フィードフォワード補償によれば、これを効果的に解決することができる。
【0115】
図12には、速度指令のフィードフォワード補償の有無によって速度応答がどうのように相違するかが示されている。速度指令のフィードフォワード補償を行えば、(a)と(b)を見比べて分かるように、破線で示す速度指令に対して実線で示すごとく追従性能(速度応答)が向上する。これから分かるように、フィードフォワードすればタイムラグが小さくなり、結果として、位置・速度・加速度の関係も実際に極めて近いものとなる。
【0116】
なお、フィードフォワード補償すると位置フィードバック比例ゲインKppの効きは落ちるが、速度はフィードフォワード的に効くので速度指令が迅速になされる。ただ速度だけでは十分な位置精度が出ないので位置をもとにした速度制御も残されており、これを速度のフィードフォワードに対して補間的に機能させておくことになる。
【0117】
ちなみに、式(2-1) はJT Kを省いたものであると説明したのは、常に外部の何物にも触れずに作業しまた空間を変位する例えばアーク溶接ロボットに適用する場合を念頭に置いたからである。そこで、本発明をワークに対して接触しながら作業する類のロボットに適用する場合は、ツールを現在位置から次の目標位置まで単に空間移動させるといったときのみ有効となる。しかし、式(2-1) にJT Kを付加しておくならば、逆動力学演算に多少の処理時間増加を伴うことになるが、力覚センサ等を働かせながらワークに対して接触する作業をしているときも本発明の思想を反映させることができるようになる。
【0118】
以上までは衝突の検出方法を述べたが、これからは衝突の検出があった後の停止処理について説明する。その処理の趣旨は、以下のとおりである。衝突が発生したときは、いち早くそれを検出すべきであることは言うまでもない。しかし、衝突を検出できた時点では衝突してから既に少し時間が経過しているわけであるから、衝突直前にロボットを停止させるということは至難の技である。
【0119】
そこで、ロボットの停止操作においては、衝突してもロボット本体やぶつかった対象物や障害物の損傷が最小限に抑えられるようにすることを最終的な目標とすべきである。簡単に言えば、各モータには、その関節を介して動かされているリンクが今持っている慣性モーメントを吸収して停止させることができる程度のトルクしか発生できないようにしておき、これによって衝突逃げ動作を可能にして被害を最小限にくい止めるとの観点に立った停止処理を行うことにする。
【0120】
この停止処理は図2のところでも触れたように、停止もしくは低速で動いている関節については、矢印9のような衝突による干渉力を和らげる方向への回転を許容できる制御をすることであり、従来技術の項で述べたように、どの関節においても可能なかぎり大きいトルクを発生させて迅速に制動しようとする場合、ロボットと障害物との間に大きな力が作用することになるというのを回避しようとするものである。
【0121】
具体的には、衝突検出できた以降は、各モータ速度を0とするために必要な最小限のトルクを越えるトルクを発生させないように、モータの発生トルクを制限することにする。例えば第3関節は止まっておりしかも重力も作用していないとするとトルクはほとんど0でよいから、ぶつかればその関節は簡単に回転し、リンクは障害物からスルッと逃げる。負荷を伴うことなく逃げることができれば、第2関節や第3関節には停止するための惰走の余地も残され、ロボット全体の干渉力や衝撃力を和らげ被害の発生が最小限に抑えられる。このように、本発明においては、大きな慣性力を持っていないかまたは高い速度が発生していない関節については、力に逆らわずに逃げる仕組みを提案しようとするものである。
【0122】
その停止処理の主旨は、関節のうち停止もしくは低速で動作している関節あるいは慣性力が小さいため動作停止に必要なトルクが最大トルクと比較して十分小さい関節については、衝突があった後に障害物に対して逃げ動作をさせ、衝突による干渉力が和らげられるようにしたことである。
【0123】
また、衝突発生時の各関節の速度と負荷イナーシャからロボットの動作を停止させるために必要な最小限のトルク値を関節ごとに算出し、その最小限のトルク値をサーボドライバ内のトルク制限器3に設定すると共に、全てのサーボドライバ内の位置フィードバック比例ゲインKppを0に置き替えるようにする。それによって、強制的に速度指令値を0とした速度フィードバック制御による減速処理を行わせるのである。
【0124】
さらに、ロボットの動作を停止させるために必要な各関節のトルク値の算出においては、全ての関節が同じ時間で停止すると仮定し、関節ごとに減速停止時間を算出した後、全ての減速停止時間のうちの最大値をいずれの関節の減速停止時間にも充てることにより、上記した逃げ動作を幾つかの関節において実現できるようにするものである。
【0125】
衝突が検出されても速度はステップ状に0となることがないので、衝突発生時の関節速度θd rはサーボドライバの状態変数であるモータ速度θd mから知ることができる。関節位置についても、サーボドライバの状態変数であるモータ位置θmから知ることができる。衝突発生時の関節位置が分かれば、負荷イナーシャであるH行列の計算は可能となる。これは当然ロボットの姿勢によって変化するイナーシャである。この衝突発生時のイナーシャと速度から以後速度を0にするに必要なトルクを計算し、この値以上のトルクが発生しないようにモータ出力トルクを制限する。このトルク出力制限処理は、図1,図9,図11に表されたトルク制限値の伝達経路28を介して行われる。
【0126】
図2のように第3関節73 は動いていないが第1関節71 と第2関節72 が動いているとすると、第1関節と第2関節についてはその慣性力を制御するだけの比較的大きなトルクが必要となるが、第3関節では慣性力は小さく従って減速に必要なトルクも第1関節や第2関節に比較すれば小さくてよい。そのため、第3関節73 を駆動するモータのトルク制限器には小さい値が設定され、第3関節に連結されたリンクが第1関節,第2関節の動きによって衝突しても、第3関節は簡単に衝突による干渉力を和らげる方向へ逃げることが可能となり、衝突による変化を吸収することができる。
【0127】
以下に、停止処理の具体例を述べる。前述したいずれかの衝突検出処理により衝突を検出した後、図13の4つのステップからなる処理を行う。まず、ステップ21において、ロボットの動作停止に使用可能なトルク値を算出する。
【0128】
衝突発生時の停止処理に使用可能な各関節のトルク値Tusabが、式(4-2) を変形した式(10-1)から算出することができる。このトルク値Tusabは、動作を停止させるための減速運動に使用できるもので、加速度によって生じるトルクと見做される。重力によるトルクや粘性によるトルクは速度に依存するものであり、加速度によるトルクとは別に確保されねばならないトルクである。
【0129】
式(4-2) を変形して速度により消失するトルク(粘性トルク)と重力により消失するトルクとを引き去れば、それが減速に供し得るトルクとなる。すなわち、遠心力や重力加速度に影響する外乱トルクによって、実際に停止のための減速運動に使用できるトルクが制限される。
【数26】
【0130】
なお、式(10-1)中のsign(θd r)は符号を決めている。Tmax は各モータが発生し得る最大トルクであって、正の値に設定される。しかし、場合によってはマイナス値として使用したいときもあるので、sign(θd r)はそれを判定している。現在の速度が正であれば負の加速度が必要となり、負のトルクが与えられる。正の速度が出ているとTmax は正となるが、負の符号がついているので負のトルクとなる。負の速度が出ていれば正の加速度が必要となるので、負×負=正となるようにしている。すなわち、速度(θd r)に対して符号を反転させることを意味している。
【0131】
−sign(θd r)・Tmax は、各関節が出力できるトルクの最大の減速トルクである。現在の速度を0にするために必要な減速トルクは、そのトルクから速度によって消失するトルクと重力によって消失するトルクを引算したもので、それが実際に減速のために使うことができる式(10-1)で与えられるトルクTusabである。
【0132】
式(10-1)は、式(4-2) のH(θr)・θddrをTusabと、式(4-2) のTを−sign(θd r)・Tmax と置き換えたものである。なお、第i関節の最大出力トルク値Tmax i は、関節を駆動するモータが出力可能な最大トルクをTm maxとすると、減速比を考慮して式(10-4)で算出することができる。言うまでもなく、Tm maxはモータの仕様から予め規定できる値であり、粘性行列Cならびに重力行列Gは、衝突発生時の各関節の位置と速度から算出可能である。
【数27】
【0133】
Tm maxi も減速比ζi も既知の値であるからTmax i も予め固定的に与えるておくことができる。C行列やG行列も衝突を検出する過程で計算されており、新たに計算する必要はない。現時点のものでよいから、式(4-2) で使ったものをそのまま持ってくればよい。本当は時間のずれが極く僅か存在するが、衝突した時から再度計算するには及ばないからである。もちろん、計算してもよいが時間を要するうえに計算値に大した違いは生じない。なお、上に現れた記号を参考までに、表7に掲記しておく。
【表7】
【0134】
ステップ22では、動作を停止させるために必要な各関節のトルク値が算出される。減速により発生するトルクは、慣性行列Hと各関節の減速度の積であり、式(11-1)で表される。なお、θddは未知であるが、分かっていればこの式によってTdec が求まることになる。
【数28】
【0135】
ところで、この停止処理の制御においては、全ての関節が時間tmdで速度0に減速停止するものと仮定する。すると、各関節の減速度θddは、式(11-2)で表現することができる。なお、式(11-2)中のθd rは、衝突発生時の各関節速度である。ただ、いまの段階ではtmdは未知である。
【数29】
【0136】
全ての関節が式(11-2)の時間tmdで減速停止することを制御条件としているのは、何らかの条件を課さなければ制御の実行が図られないためであるが、この条件によって、幾つかの関節において上記した逃げ動作を可能ならしめることができるようになるのである。
【0137】
そこで、時間tmdとして、後で説明する式(11-4)ないし式(11-6)でロボットの関節の数だけ求まる複数のtmdの中から、最長のものを使用することにする(式(11-7)を参照)。これは、ロボットの全ての関節において衝突後のロボットの減速運動により発生するトルクが、減速に使用可能なトルクTusabを越えないようにしておくためであり、それを実現するtmdによって、全ての関節を同じ時間で停止させることができるようになる。
【0138】
なお、参考までに記せば、選定されたtmdの関節は、結果的に最も速く動いている関節あるいは一番大きいモーメントを受ける関節であることが多い。ちなみに、このようにして選定された時間によれば、最も遅く動いている関節では速度が0になるまでに時間が十分にあり無用な惰走が強られることになるが、元来速度は低いので多少惰走してもその距離がさして大きくなることはない。
【0139】
ここで、停止処理の減速に使用できるトルクは、式(10-1)からTusabと算出されているので、式(11-1)の右辺をTusabに置き換え、全ての関節が式(11-2)の時間tmdで減速停止するものとする。式(11-1)中のθddに式(11-2)の右辺を代入すると、式(11-1)は式(11-3)のように変形される。式(11-3)中の慣性行列Hの算出方法は、既に説明したとおりである。
【数30】
【0140】
この式(11-3)から、停止処理の減速に使用できるトルクTusabの範囲内で、ロボットの各関節の速度を0にできる減速停止時間tmdが算出できる。なぜなら、式(11-3)において、H(θr)は、衝突発生時の各関節位置θrから算出できかつθrはサーボIF20を介してモニタ可能であるからである。また、θd rは衝突発生時の各関節速度でありサーボIFを介してモニタ可能であり、Tusabは式(10-1)で計算済みでもある。
【0141】
最終的には未知の値はtmdだけとなる。tmdは行列式であり、3つの関節からなるロボットの場合には、3つの関係式ができる。実際には、式(11-3)を、関節ごとに以下の3つの式に分解し、減速停止時間tmdを算出する。具体的に言えば本実施例では関節数n=3であるから、関節ごとに減速停止時間tmd1 ないしtmd3 を算出した後に、tmd1 ,tmd2 ,tmd3 の中の最大値を全ての関節の減速停止時間に充てる。式(11-7)中のMAXは要素中の最大値を抽出することを意味している。
【数31】
【0142】
動作を停止させるために必要な各関節のトルク値Tdec i は、式(11-4)ないし式(11-7)から算出される減速停止時間tmdを用いて、式(11-8)で算出される。なお、Tdec i は停止に必要な関節のトルク値 [N・m] で、その個数はn(=3)×1行列である。tmdは全ての関節での減速停止時間 [sec] で、個数は1である。
【数32】
【0143】
このようにして算出されたTdec は、衝突発生時のロボットの各関節の位置・速度により変化する負荷イナーシャや外乱トルクの変動を考慮したもので、停止時間に遅延を生じさせない必要最小限のトルク値となる。
【0144】
式(11-1)のH行列,式(11-2)の現在の関節速度θd rおよび減速停止時間tmdから、各関節に必要な減速トルクTdec が式(11-8)により分かる。どこか一つの関節すなわち減速動作に最大時間を要すると思われる関節についてはTusabに等しくなるが、他の関節ではTdec <Tusabとなる。これは速度が小さいか慣性力(=H)が小さいものについてはTdec も小さくなるからで、これがトルク制限値として設定されると、干渉力に対して逃げ動作を可能にする。
【0145】
ステップ23では、サーボドライバ2内のトルク制限器3の設定値が変更される。すなわち、ステップ2で算出したTdec は関節で発生するトルクであり、実際にモータで発生するトルクの制限処理を行うのはサーボドライバで行われる。通常、モータは減速機を介して関節を駆動しているので、Tdec をモータシャフトの換算値Tm decに変更し、サーボIF20を介した伝達経路28を通じてTm decが各サーボドライバのトルク制限器3に設定される。
【0146】
第i関節を駆動するモータにおける停止時間に遅延を生じさせない必要最小限のトルク値Tm deciは式(12-1)のようにして算出することができる。なお、Tm decは停止に必要なモータのトルク値 [N・m] であり、その個数はn(=3)×1行列となる。
【数33】
【0147】
ちなみに、トルク制限値として式(11-8)のTdec がサーボドライバに入ってくるが、Tdec はそのまま入れられない。動力学演算というのは一般的に関節レベルで得られるので、求まったTdec も関節レベルのトルクであって、実際にトルクを制限するのはモータである。モータと関節との間には減速機が介在されているから減速比で割った値を設定しなければならない。従って実際はTm deci が入れられることになる。
【0148】
念のために記載するが、H行列やθd rが関節ごとに違うから、tmd1 =tmd2 =tmd3 としているとはいえ、Tm deci はサーボドライバ21 ,22 ,23 ごとに異なる。なお、得られたTmdec i をそのまま使用してもよいが、Tmdec i に対して関節ごとにウエイトづけすれば都合のよい場合がある。そのような場合には、異なった係数を各Tmdec i に乗じて採用すればよい。
【0149】
ステップ24では、各サーボドライバ2内の位置フィードバック比例ゲインKppが、サーボIF20を介して0に変更される。以上のステップ21ないし24の処理により、強制的に速度指令値を0とした速度フィードバック制御による減速処理が行われる。
【0150】
各サーボドライバ内のトルク制限器3には、停止に必要な最小限のトルク値が設定されるので、衝突による干渉力がこれを上回る場合、関節が干渉する力(衝突力)を逃がす方向に動作し衝撃が軽減される。トルク制限器に最大トルク値が設定される従来の場合に比べれば、干渉力緩和効果が飛躍的に改善されることになる。
【0151】
このように、トルク制限値を置き替えるだけでなくKppを0にしているのは、トルク制限値の置き替えだけでは関節が止まらないからである。位置指令や位置のフィードバックに如何なる値が来ていようとも、Kppを0にすれば速度指令は直ちに0となる。速度指令を単純に0とするには、Kppを0にすることが手っとり早いからである。
【0152】
上記から分かるように、速度ループだけで関節の動きを止めにいこうとするのであるが、トルクが適当に変更して制限されているので、速く動いている関節では最大トルクで止める制御がなされる。一方、他の関節では、自己の速度を0に保持するだけのすなわち現在の姿勢を維持するだけのトルクしか発生しないようにしておくことができる。
【0153】
これから分かるように、上記の停止処理によれば、トルク制限値を越える干渉力や衝撃が作用した場合に、その力の優劣を利用して関節に力逃しの挙動をさせることができ、ぶつかったことによる損傷の発生が可及的に抑えられる。なお、停止した状態でも、障害物にぶつかった関節であれぶつかっていない関節であれ速度ループにおける積分項はいつまでも機能するので、各リンクは関節が発揮している制限値内のトルクによって姿勢を維持し続ける。
【0154】
ちなみに、例えば水平面内のみで変位するリンクであって、姿勢保持に重力に対抗する力を必要としないロボットの場合やトルクが掛かり放しでは不味い境遇にあるリンクなどでは、速度が0になった時点でトルクを解放すればよい。その場合にはKviの入っている積分項の働きをなくせばよく、Kppを0にするときもしくはその後に積分項をクリアする操作を付加しておけばよい。
【0155】
以上で、衝突検出後の停止処理の説明を終えて、以下に、衝突検出精度の向上策について述べる。既に述べたように、トルク指令値に代えてトルク応答値を採用することもその一つであるが、ここではモータロータの慣性モーメントを考慮したり、モータ軸受や減速機の摩擦抵抗を考慮することによる衝突検出精度の向上について触れる。いずれも単独もしくは重複して機能させることができるものである。
【0156】
まず、モータロータの慣性モーメントの考慮による衝突検出精度の向上から説明する。これは、前記したn×nの慣性行列における対角要素としての「関節の持つ加速度が該関節自体に及ぼすトルクの関係を示した慣性モーメント」、すなわち「関節位置により変動する負荷イナーシャ」に、減速機を含むモータロータの慣性モーメントを減速比の2乗倍して加算しようとするものである。これによって、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクも考慮して、衝突検出精度を向上させることができる。
【0157】
まず、式(2-9) の慣性行列Hを以下のように変形することにより、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮に入れる。ここで、Imはモータロータの慣性モーメント [kg・m2]で、その個数はn(=3)×1行列となる。
【数34】
【0158】
図1や図9さらには図11の例に共通して言えることであるが、慣性行列Hは各関節に連結されたリンクのイナーシャだけを計算しているに過ぎない。関節はモータで駆動されるから動くのであるが、関節を駆動する以前にモータも駆動されており、そのときのロータ自体の回転の慣性にも注目すべきである。
【0159】
ここで、Imi は減速機を含むモータロータの慣性モーメントである。連動して動く回転体の慣性モーメントは、その減速比の2乗分の1倍されて入力軸の慣性モーメントに影響を与える。従って、入力軸の慣性モーメントが出力軸に与える影響はその逆であるため、減速比の2乗倍されなければならない。減速機を含むモータロータの慣性モーメントが減速比の2乗倍されて加算されているのはそのためである。
【0160】
慣性行列Hの対角要素はそれ自体に与えるトルクの影響を示し、他の要素は他の関節に与える干渉トルクの影響を示す。減速機を含むモータロータの慣性モーメントが他の関節と干渉することはないので、対角要素のみを修正すればよい。なお、Imi もξi も予め設定可能な既知であるから、式(13-1)による場合も、既に述べた例と同様に衝突検出ならびに停止処理は可能である。
【0161】
次に、摩擦抵抗を考慮して衝突検出の精度を上げることについて説明する。式(2-1) で表されるロボットダイナミクスモデルを、式(14-1)のように変更することにより、各関節内のモータの軸受や減速機の摩擦抵抗を考慮できるようにしようとするものである。なお、ここで現れる記号を参考までに、表8に掲記しておく。
【数35】
【表8】
【0162】
減速機自体にも摩擦がかなりあり、これも無視し得ない。速度項のことを粘性行列というが、それがCm・θd である。しかし、これは物が擦れるときの摩擦を対象とするものではない。或る速度で動いているものが或る速度で動いているものに重なると、その速度と速度が干渉しあって別方向にトルクが生じるが、そのトルクを意味する。これは、式(14-1)の4項目に表されている。
【0163】
Cmは動粘性係数であり、これに速度を乗じれば、外乱トルクとなる。|Fcou |はクーロン摩擦(静摩擦:動き始めるまでの抵抗)と言われるもので、速度に依存することなく一定の大きさを持つが、常に運動と逆の方向に働くトルク成分である。Cm・θd mと|Fcou |との和が、ここで考慮すべき摩擦トルクである。式(14-1)は式(2-1) に4項目を追加したものであり、摩擦を考慮した演算で精度を上げようとするものである。
【0164】
ここで、Cmはモータの軸受けや減速機の動粘性係数n×1行列であり、動粘性係数とは、速度と動摩擦トルクとの比例関係を表す定数である。また、動摩擦トルクとは、速度に比例した大きさを持つトルク成分である。さらに、θd mはモータの速度n×1行列である。なお、式(14-1)中のsgn ( θd m)は、モータの速度に基づいて、次式のように決定される符号関数である。
【数36】
【0165】
また、関節速度とモータ速度の関係はその減速比ξから、式(14-5)のように表される。
【数37】
ここで、ξは関節内の減速機の減速比である。パラメータCm,Fcou ,ξも予め設定可能な既知の値であるから、上記(14-1)によっても衝突検出ならびに停止処理できることは述べるまでもない。
【0166】
【発明の効果】
本発明によれば、サーボドライバが検出したモータの位置・速度・加速度から衝突が発生していない場合に各モータが発生すべきトルクを予測し、そのトルク予測値と実際の関節駆動トルクとの差分を求め、その絶対値が予め設定された判定値より大きい場合に衝突が発生したものと見做すようにしたので、ロボットのいずれかのリンクやツールがワーク等の対象物やその他の障害物に衝突したことをトルクを通じていち早くかつ正確に検出することができるようになる。
【0167】
衝突が生じていない場合に各関節を駆動するモータが発生すべきトルクを、ロボットの各関節の位置θ、速度θd 、加速度θddとロボットの機構パラメータとをもとにして、T=H(θ)・θdd+C(θ,θd )+G(θ)・gの逆動力学演算によって予測するようにしているので、そのトルク予測値を高精度に求めておくことができ、衝突センサ等の機械的構成物を搭載しなくても、迅速かつ正確に衝突を検知できる。
【0168】
関節のうち停止もしくは低速で動作している関節あるいは慣性力が小さいため動作停止に必要なトルクが最大トルクと比較して十分小さい関節には、衝突があった後に逃げ動作させるようにしておくので、衝突による干渉力を軽減して物損の発生を抑制することができる。
【0169】
衝突発生時の各関節の速度と負荷イナーシャからロボットの動作を停止させるために必要な最小限のトルク値を関節ごとに算出し、その最小限のトルク値をサーボドライバ内のトルク制限器に設定すると共に全てのサーボドライバ内の位置フィードバック比例ゲインKppを0とし、強制的に速度指令値を0とした速度フィードバック制御による減速処理を行わせるようにすれば、位置指令や位置のフィードバックに如何なる値が来ようとも、速度指令を単純に0にしておくことができ、加えて、衝突による干渉力がトルク制限値を上回る場合、関節が干渉力を逃がす方向に動作して衝突力が軽減される。
【0170】
ロボットの動作を停止させるために必要となる各関節のトルク値の算出においては、全ての関節が同じ時間で停止すると仮定し、関節ごとに減速停止時間を算出した後、全ての減速停止時間のうちの最大値をいずれの関節の減速停止時間にも充てるようにすれば、衝突しても逃げ動作する余地が残され、衝突による力を吸収して損傷の発生を可及的に少なく抑えることができる。
【0171】
トルクを予測する際に使用するモータの位置・速度・加速度として、サーボドライバに位置指令すべく作業データの再生動作において演算した関節位置と、その関節位置をもとにして計算された速度および加速度をあてがうようにすれば、サーボドライバからモータの位置・速度・加速度の情報を取り込む時間等だけ迅速に衝突検出演算を行わせることができる。
【0172】
逆動力学演算においては、ロボットの各関節の加速度により各関節で発生するトルクならびに他の関節の加速度により発生する干渉トルクを示すn×nの慣性行列、遠心力とコリオリ力の影響を示すn×1の粘性行列、および重力加速度の影響により発生するトルクを示すn×3の重力行列を含むことにしているので、その演算精度は極めて高いものとなる。
【0173】
n×nの慣性行列における対角要素としての「関節の持つ加速度が該関節自体に及ぼすトルクの関係を示した慣性モーメント」に、減速機を含むモータロータの慣性モーメントを減速比の2乗倍して加算しておけば、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮した衝突検出を行うことができる。
【0174】
逆動力学演算に供される式に、減速機の動粘性係数とクーロン摩擦トルクの要素を反映させるようにしておくと、各関節内のモータ軸受や減速機の摩擦抵抗を考慮した精度の高い衝突検出が実現される。
【0175】
逆動力学演算に供されるロボットの機構パラメータとして、関節に連結されるリンクの重心位置・質量・長さを与えておけば、演算が個々のロボットに則したものとなり、ましてや、慣性テンソルも付加しておけば、より一層精度のよいトルク予測が可能となる。
【0176】
トルク予測値との差分が求められる実際の関節駆動トルクとして実際にサーボドライバが生成したトルク指令値から算出されるトルクを使用すれば、演算速度を高く維持することができる。各モータにおけるトルク応答値から算出されるトルクとする場合には、現実のトルクとの比較になって衝突検出のための精度がさらに向上する。
【図面の簡単な説明】
【図1】 本発明に係る多関節ロボットにおける衝突検出・停止制御法が適用されているロボット制御系全体を示したブロック線図。
【図2】 関節数を3として簡略化した構造のロボットを表すと共に、リンクが障害物に当たったときの逃げ動作を表したロボットの概略図。
【図3】 ロボットコントローラ(制御装置)とティーチペンダントならびに関節駆動用モータとの間の信号伝達の概要を示した制御ブロック図。
【図4】 衝突の有無を判定する処理手順を表したフローチャート。
【図5】 イナーシャテンソルの基準座標を与えた第2リンクの説明図。
【図6】 サーボドライバからコントローラへ伝達される応答値の変更例を含んだ制御系全体を示すブロック線図。
【図7】 トルク指令値に代えてトルク応答値を採用する場合のサーボドライバのブロック線図。
【図8】 ブラシレスDCモータを使用している場合のサーボドライバのブロック線図。
【図9】 軌道計画によって生成された関節位置から得られる速度・加速度をもとにして逆動力学演算させるようにしたロボット制御系全体を示すブロック線図。
【図10】 衝突の有無を判定するにおいて図9に基づく処理手順を採用した場合のフローチャート。
【図11】 速度をサーボドライバにフィードフォワードした場合のロボット制御系全体のブロック線図。
【図12】 速度指令に対する速度応答の遅れの程度を表したものであり、(a)は速度をフィードフォワードしない場合の応答図、(b)はフィードフォワードした場合の応答図。
【図13】 停止処理する手順を表したフローチャート。
【図14】 モータを駆動するための各種制御ループが組み込まれたサーボドライバの構成概略図。
【図15】 サーボドライバから位置偏差を取り出して衝突の有無を判定すると共に、ひき続いて停止処理することもできるようになっていることを表した従来技術のブロック線図。
【符号の説明】
1,11 ,12 ,13 …モータ、2,21 ,22 ,23 …サーボドライバ、3…トルク制限器、71 …第1関節、72 …第2関節、73 …第3関節、8…障害物、10…ロボット、11…衝突検出部、12…停止処理部、13…コントローラ、15…ハードディスク、22…軌道計画部、23…電流検出器、25…速度・加速度計算部、291 ,292 ,293 …比較器、L1 …第1リンク、L2 …第2リンク、L3 …第3リンク、θm1 ,θm2 ,θm3 …サーボドライバが検出したモータの位置、θd 1 ,θd 2 ,θd 3 …サーボドライバが検出したモータの速度、θdd1 ,θdd2 ,θdd3 …サーボドライバが検出したモータの加速度、T1 ,T2 ,T3 …衝突が発生していない場合に各モータが発生しているトルク予測値、Tm1 ,Tm2 ,Tm3 …サーボドライバが実際に生成したトルク指令値、ΔT1 ,ΔT2 ,ΔT3 …トルク予測値とトルク指令値から算出されるトルクとの差分、ε1 ,ε2 ,ε3 …衝突判定値、Kpp…位置フィードバック比例ゲイン。
[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a collision detection / stop control method for an articulated robot, and more specifically, if a robot collides with an external object such as a workpiece or an obstacle while the robot is in operation, the collision is detected quickly and accurately, and thereafter The present invention relates to a robot control method that can quickly stop the movement of the robot or release the force to avoid deformation and damage of the robot body and tools.
[0002]
[Prior art]
When a robot link consisting of multiple joints or a tool or work gripped at the tip of the robot is replaying based on teaching data or moving by a teaching operation using the teach pendant, the workpiece is unintentionally processed You may hit an object or a nearby obstacle. If replaying is in progress, the motors that drive each joint of the robot will follow the command values, and if teaching continues without being aware of the collision even during teaching, the robot will naturally move even if it breaks through obstacles etc. And As a result, an excessive torque is generated depending on the joint, and accordingly, a link or a tool is deformed or a movable part of the robot such as the joint is damaged.
[0003]
For example, as shown in FIG. 14, a position and speed control system is incorporated in each servo driver for controlling each motor that executes a reproduction operation of work data taught to the robot. The operation of the motor 1 is controlled so as to follow the position command given from the robot controller), and the reproduction operation of the work data is executed. Each servo driver 2 is provided with a torque limiter 3 after the speed control system so that a torque command generated by the speed control can be limited by an arbitrary value.
[0004]
As can be seen from this control system, the presence or absence of a collision can be detected by monitoring the error component of the position feedback system that is the outermost loop, that is, the magnitude (position deviation) obtained by subtracting the position feedback value from the position command. be able to. As shown in FIG. 15, the most basic one is that a position deviation obtained as a result of feedback control of the position of the motor 1 is taken out immediately after the adder 4 in the position control loop. If it is larger than the determination value 6 for each joint, which is a “threshold value”, it is determined that a collision has occurred.
[0005]
According to such control, when the shock sensor torch hits, the switch is turned on and the servo power supply is turned off, or compared with the existing devices such as the detection device equipped with a mechanical collision detection mechanism, It is possible to detect a collision only by installing the collision detection software in the robot controller even if the motor, which is the minimum necessary component of the robot, is provided, that is, without the collision sensor. Therefore, the mechanism of the robot body can be prevented from becoming complicated.
[0006]
As another method for detecting a collision, there is a collision detection method using a disturbance estimation observer described in Japanese Patent No. 2,749,724. This was caused by a collision with an observer that observes disturbance torque in each servo driver, estimating the size of the inertia that the motor is actually trying to rotate by using an internal model. The disturbance torque is detected, and the presence or absence of a collision is determined by looking at the magnitude of the estimated value of the disturbance torque.
[0007]
That is, the idea is to predict what kind of behavior is made for a given command value by modeling inside, and to make a pre-read compensation by matching the predicted behavior with the actual behavior. Therefore, if the inertia can be modeled, the torque can be predicted, and if there is no fluctuation in the inertia, a collision can be detected with high accuracy. This is because inertia modeling is easy if only a single joint is used, regardless of modeling with a plurality of joints.
[0008]
In this way, the collision can be detected only when there is a change in the servo state variable due to the collision, and the collision is sensed by the torque variation detected in each motor unit.
[0009]
By the way, it is necessary to stop the movement of the robot after detecting a collision, but in order to stop it, from the idea that it is possible to brake quickly by generating as much torque as possible at any joint, Generally, a maximum torque value is set in the torque limiter. Needless to say, the maximum torque means the maximum torque that can be generated in each motor.
[0010]
Specifically, by setting the position feedback gain to 0, the command to the speed control system is forcibly set to 0, and the stop process is performed by performing speed feedback control with the speed 0 as a target value. The torque used for deceleration for stopping is always possible up to the maximum allowable torque of the motor by a torque limiter arranged after the speed control system.
[0011]
[Problems to be solved by the invention]
However, in the above-described collision detection system of FIG. 15, it is impossible to recognize that a collision has been detected unless error components at any joint position are sufficiently accumulated. Therefore, when the collision is detected, the motor has already generated a large torque, and the deformation and damage of the robot main body and the tool cannot be reduced as much as possible. That is, even if a collision can be detected, it is too late at that time.
[0012]
For example, the third joint 7 shown in FIG. Three Is the third link L Three Even if an attempt is made to maintain a straight posture as shown by a broken line, for example, the second joint 7 2 If it is pushed by, hits the obstacle 8. Third link L Three Tries to be moved as shown by the solid line by the obstacle 8, but the feedback control is effective, so the third joint L Three Motor 1 driving Three Tries to return straight to maximum position with maximum torque. Naturally, the interference force increases and a large force interacts.
[0013]
As shown in FIG. 15, the servo driver 2 has torque control, speed control on the outer side, and position control on the outermost side. Therefore, the torque is most sensitive to the collision. Next is speed, and the most insensitive is the position. Therefore, if the collision is detected by the position deviation, the following drawbacks cannot be covered.
[0014]
In feedback control, there is always a positional deviation to generate speed and torque, and it goes without saying that it increases when operating at high speed and with large acceleration. On the other hand, when moving slowly, the positional deviation is small even when hitting an obstacle.
[0015]
Therefore, if it is determined that the collision is caused by a small position deviation, a small position deviation is immediately exceeded when operating at a high speed and a large acceleration, so the collision is often erroneously detected and the robot just stops. It becomes useless. On the other hand, if it is determined that the collision is caused by a large positional deviation, the collision cannot be detected unless it goes too far when moving at a low speed and a small acceleration.
[0016]
As can be seen from this, it is inevitable to detect a collision based on the position deviation because it is necessary to enter a large setting value that does not misrecognize as a collision even if the strictest possible movement is considered. End up.
[0017]
When a disturbance estimation observer is used as in Japanese Patent No. 2,749,724, the observer is generally incorporated in a velocity loop. Since this is a story in a servo driver, a disturbance is detected at each joint level. Inertia modeling is easy if only a single joint is handled, but modeling with multiple joints is extremely difficult, and for multi-joint robots it is possible to ensure sufficiently high collision detection accuracy. It can be said that it is difficult.
[0018]
A robot composed of a plurality of joints has a characteristic that even if an arbitrary motor that drives the joint is operated in the same speed pattern, the torque generated by the motor varies depending on the posture of the robot. This is because the load inertia to be driven by the motor and the disturbance torque received by the motor vary depending on the joint position of the robot, that is, the posture of the robot and the speed / acceleration state of each joint.
[0019]
Needless to say, the robot gets more inertia when the arm is extended, and it gets smaller when the arm is shortened. In the case of using a disturbance estimation observer, since high-order control is performed, it is practically impossible to define an observer with a plurality of joints. As an item required for the accuracy of the observer, there is interference between joints caused by centrifugal force, Coriolis force and the like. The square terms of these velocities do not have a significant effect with a single joint, but the effect becomes large when the joint consists of a plurality of joints. Disturbance torque due to gravitational acceleration also changes as the robot posture changes. As the turning radius changes, so does the moment due to gravity.
[0020]
A collision can be detected only when there is a change in the servo state variable due to the collision. Since the disturbance estimation observer is built into the servo driver, disturbance detection is only at the driver level, so it is the torque fluctuation detected by each motor that senses the collision, and it is used to detect disturbance at each joint level. Will stay.
[0021]
As a matter of fact, it is necessary to know what control object exists at the tip of the motor, but even if disturbance torque is detected at one joint level, it can be detected normally if there is no inertia fluctuation. In an articulated robot where there is inertia fluctuation or inertia fluctuation is unavoidable, it can be distinguished from what happened to occur when the torque fluctuation occurred, or whether it occurred due to a collision. I want. That is, when there are a plurality of joints, in addition to the fluctuation of the load inertia described above, the following three disturbance torques exist, and the determination is difficult.
[0022]
The types of disturbance torque received by each joint of a robot composed of a plurality of joints are the following three. One is the interference torque received from the acceleration motion of other joints (changes depending on the robot joint position and acceleration), and the second is the disturbance torque due to centrifugal force and Coriolis force (changes depending on the robot joint position and speed). ). The third is disturbance torque due to gravitational acceleration (which varies depending on the joint position of the robot).
[0023]
As described above, the torque is the most sensitive in detecting a collision, but even a disturbance estimation observer that uses this as a detection target cannot grasp the inertia fluctuation. This is because it functions for each joint, so it is impossible to consider the load inertia and disturbance torque fluctuations linked to the state of the motor that drives the other joints, and accurately extract only the disturbance torque generated by the collision. It will be difficult to do. In view of such circumstances, it must be said that there are still some problems to be solved in the case of an articulated robot.
[0024]
By the way, as described above, when the robot is stopped after the collision, speed feedback control is performed with a target of speed 0. However, since the integral term is functioning, the torque is still generated and it is increased. Inevitable. This is because it is generally said that the maximum torque can be exerted on the motor, and if the motor exerts a large force, a large force is inevitably applied between the robot and the obstacle. is there. That is, the escape operation when there is interference is hindered by the maximum torque, and there is a problem that the interference force due to the collision cannot be reduced at all.
[0025]
Incidentally, the torque limiter cannot be set to 0 when a collision occurs. Even if it is hit, there is inertial force, so only braking is applied to the inertial force, and the braking force is significantly reduced. In addition, even for a joint that is stopped, it is impossible to brake the joint motion due to the impact force caused by a collision or interference torque received from another joint.
[0026]
The present invention has been made in view of the above-described problems, and an object thereof is to provide a collision detection / stop control method in an articulated robot that can largely overcome the following two problems. The first problem is that, even in a robot composed of a plurality of joints whose joint drive torque varies nonlinearly, it is possible to detect in a short time that the collision has occurred due to the fluctuation of the joint drive torque generated by the collision. is there. That is, it is the torque fluctuation detected for each motor that senses the collision, but it tries to be able to determine whether or not it is due to the collision by calculating as strictly as possible.
[0027]
The second problem is to mitigate the interference force from the obstacle due to the collision in the stop process after the collision detection of the robot, and to minimize the damage to the robot main body and the object to be hit. In other words, for a joint that does not have a large inertial force or a joint that does not generate speed, it intends to propose a mechanism for escaping in the direction of force.
[0028]
[Means for Solving the Problems]
In the present invention, when a link of a robot composed of a plurality of joints, a tool or a work held at the tip of the link collides with an obstacle, etc., it collides to avoid deformation of the link or damage to the robot movable part. This method is applied to a method of detecting the robot and immediately stopping the robot. The feature is that referring to FIG. 1, each joint driving motor 1 of the robot. 1 , 1 2 , 1 Three Servo driver 2 for controlling 1 , 2 2 , 2 Three Detected motor position θm and speed θ d m ・ Acceleration θ dd m, the torque T that each motor should generate when no collision has occurred. 1 , T 2 , T Three The position θ and velocity θ of each joint of the robot d , Acceleration θ dd And robot mechanism parameters
[Equation 3]
Predicted by inverse dynamics calculation based on the formula A difference ΔT between the predicted torque value and the actual joint driving torque Tc is obtained. It is considered that a collision has occurred when the absolute value of the difference ΔT is larger than a preset determination value ε. Of the joints, a joint that is stopped or operating at a low speed, or a joint that has a sufficiently small torque compared to the maximum torque because the inertial force is small. It was to be able to escape to ease.
[0029]
When stopping the robot, the minimum torque value required to stop the robot operation is calculated for each joint from the speed and load inertia of each joint at the time of the collision, and the minimum torque value is servoed. The torque limiter 3 of the driver is set, and the position feedback proportional gain Kpp of all servo drivers is set to 0, and the deceleration process by the speed feedback control with the speed command value set to 0 is performed forcibly.
[0030]
In calculating the torque value of each joint necessary for stopping the robot operation, it is assumed that all joints stop at the same time, and the deceleration stop time tmd for each joint. 1 , Tmd 2 , Tmd Three It is preferable that the maximum value of all deceleration stop times is used for the deceleration stop time of any joint.
[0031]
As shown in FIG. 9, the servo driver 2 is used as the position / speed / acceleration of the motor used to predict the torque to be generated by the motor that drives each joint when no collision occurs. 1 , 2 2 , 2 Three The joint position θ calculated by the reproduction operation of work data taught in advance to give a position command to the position and the speed θ calculated based on the joint position d And acceleration θ dd May be adopted.
[0032]
In the inverse dynamics calculation, if n is the number of joints of the robot, n × n inertia matrix indicating the torque generated at each joint by the acceleration of each joint of the robot and the interference torque generated by the acceleration of the other joints, An n × 1 viscosity matrix indicating the influence of centrifugal force and Coriolis force and an n × 3 gravity matrix indicating the torque generated by the influence of gravity acceleration are included.
[0033]
In the “inertia moment indicating the relationship of the torque that the acceleration of the joint exerts on the joint itself” as a diagonal element in the n × n inertia matrix, the inertia moment of the motor rotor including the reduction gear is the square of the reduction ratio. Thus, the torque generated by the moment of inertia of the motor rotor including the speed reducer in each joint can be taken into consideration.
[0034]
In the formula used for inverse dynamics calculation,
[Expression 4]
However, Cm i Kinematic viscosity coefficient of reducer at joint i
(Same for i = 1, 2,... N)
θ d m i Speed response of motor at joint i
Fcou i Coulomb friction torque of reducer at joint i
sgn (θ d m i Sign function
= 1; θ d When m> 0
= 0; θ d When m = 0
= -1; θ d When m <0
Is preferably added so that the frictional resistance of motor bearings and reduction gears in each joint can be taken into consideration.
[0035]
The mechanism parameters of the robot need only include the center-of-gravity position, mass, and length of the link connected to each joint. Furthermore, it is even more preferable if an inertia tensor is also added.
[0036]
The actual joint drive torque for obtaining the difference from the predicted torque value may be the torque calculated from the torque command value actually generated by the servo driver.
[0037]
If the torque calculated from the torque response value in each motor is employed, the collision detection accuracy can be further enhanced.
[0038]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, a collision detection / stop control method in an articulated robot according to the present invention will be described in detail with reference to the drawings showing embodiments thereof. First, an outline of the configuration of an arc welding robot as an example to which the present invention is applied, and control of a motor that drives a joint connecting links will be described. In the present invention, when a robot collides with an external object such as a workpiece or other obstacles during work, the robot detects the collision quickly and accurately, and then stops the movement of the robot promptly. It is intended to release all or part of the force acting on the link so as to reduce or avoid deformation or damage of the robot body, tool or workpiece.
[0039]
The details will be described later with reference to the drawings and tables, but the base of the present invention will be described with reference to FIG. 2 schematically showing the escape operation of any link in a robot composed of a plurality of joints. . This example shows three joints 7 1 , 7 2 , 7 Three The first joint 7 1 And second joint 7 2 3rd link L during high speed operation Three Represents the case of collision with the obstacle 8. Needless to say, the third joint 7 is stopped or moves little as indicated by an arrow 9 in the figure. Three If the robot rotates in a direction that reduces the interference force caused by the collision, the impact caused by the collision of the entire robot can be greatly reduced.
[0040]
By the way, as mentioned in the section of the prior art, in a robot composed of a plurality of joints whose joint driving torque fluctuates nonlinearly, each joint is generated in consideration of the following factors even when no collision occurs. It is important to accurately predict the torque value. This is because if a change in joint drive torque caused by a collision is detected based on them, the occurrence of the collision can be detected with high accuracy and in a short time.
[0041]
The factors are: (1) Load inertia fluctuation caused by changes in the position of each robot joint (referred to as “inertia fluctuation”), and (2) acceleration movements of other joints caused by changes in the position and acceleration of each robot joint. Interference torque fluctuation (referred to as “acceleration interference”), (3) Disturbance torque fluctuation (called “velocity interference”) consisting of centrifugal force and Coriolis force caused by changes in the position and speed of each joint of the robot (4) ) Disturbance torque fluctuations (called “gravity interference”) due to gravitational acceleration caused by changes in the position of each joint in the robot.
[0042]
A control block diagram illustrating the first example of the present invention is represented in FIG. The link L of the robot 10 shown in FIG. 1 , L 2 , L Three In order to prevent the link or tool from being deformed when a tool or work (not shown) gripped at the tip of the link collides with the surrounding obstacle 8, and further to prevent damage to the robot movable part such as a joint. The collision detecting unit 11 in FIG. 1 detects a collision, and the stop processing unit 12 can immediately stop the robot. Motor 1 for each of the three joints 1 , 1 2 , 1 Three Servo driver 2 that operates in response to a command from robot controller 13 for each motor. 1 , 2 2 , 2 Three Has been prepared.
[0043]
The main part of the collision detection process in the control according to the present invention includes the following steps. First, a servo driver 2 that controls each motor for driving the joints of the robot. 1 , 2 2 , 2 Three Detected motor position θm 1 , Θm 2 , Θm Three , Speed θ d m 1 , Θ d m 2 , Θ d m Three , Acceleration θ dd m 1 , Θ dd m 2 , Θ dd m Three From the torque T that each motor should have generated when no collision has occurred. 1 , T 2 , T Three Is predicted by inverse dynamics calculation described later.
[0044]
Next, the predicted torque value T 1 , T 2 , T Three And the actual joint drive torque, ie servo driver 2 1 , 2 2 , 2 Three Is the torque command value Tm actually generated 1 , Tm 2 , Tm Three The difference ΔT from the torque calculated from 1 , ΔT 2 , ΔT Three Ask for. And any difference ΔT i The corresponding judgment value ε, whose absolute value is preset i If it is larger, it is assumed that a collision has occurred.
[0045]
It has already been mentioned that collision detection is possible only when there is a change in the servo state variable due to a collision. He also mentioned that it is the torque fluctuation detected by each motor that senses the collision. Whether the torque fluctuation is caused by the occurrence of the torque or by the collision cannot be determined unless the existence of the four disturbance torques described above is determined when there are a plurality of joints. However, in order to determine its existence, a strict calculation is required.
[0046]
Therefore, the torque to be generated at each joint necessary for the robot to perform a desired motion is obtained, and this is matched with the torque actually generated by the motor, and the presence or absence of a collision is detected from the difference. Although there is a tendency for the amount of calculation to increase, detection becomes even more accurate. In this example, the current position / speed / acceleration of the motor is taken from the servo driver at any time after the motor is energized, and the torque value that should have been output to each motor is calculated by using them. Is.
[0047]
Here, first, the configuration of the robot system will be described with reference to FIG. The robot includes an appropriate number of joints depending on the application, but a robot composed of three joints is taken as an example as already shown in order to simplify the drawing. Motor 1 that drives each joint of robot 1 , 1 2 , 1 Three Each of the encoders 1e provides position information as the amount of forward / backward movement by rotation. 1 , 1e 2 , 1e Three Is attached.
[0048]
A control device (robot controller) 13 for driving these motors is provided in a control box separate from the robot body, stores teaching data instructed by the teach pendant 14, and is stored in advance as its storage data. An operation for reproducing the motion is performed based on the mechanism parameters unique to the robot.
[0049]
Between the controller 13 and the motor 1, a servo driver 2 for controlling the position and speed of each motor is provided. 1 , 2 2 , 2 Three And servo amplifier 2a for supplying electric power to the motor based on the command current value obtained by the position / speed control. 1 , 2a 2 , 2a Three Is also provided. Incidentally, the configuration of the servo driver 2 is as shown in FIG. 14 described in the section of the prior art, and is mounted in the form described in the lower half of FIG.
[0050]
Each servo driver incorporates a position control loop and a speed control loop in order to control the motor to follow the position command given from the control device. The position control loop performs feedback control using a proportional element, and the speed control loop performs feedback using a proportional element and an integral element. The position feedback proportional gain Kpp can be changed to an arbitrary value in response to a request from the robot controller.
[0051]
A torque limiter 3 is disposed after the speed control system, and a torque command value Tm generated by the speed control. i Can be limited by an arbitrary value. The limit value set in the torque limiter 3 can also be changed to an arbitrary value in response to a request from the controller 13. In the figure, Kvp is a speed feedback proportional gain, Kvi is a speed feedback integral gain, S is a differential element, and 1 / S is an integral element.
[0052]
The controller 13 issues control commands for the three joints. As roughly shown in FIG. 3, the central processing unit CPU, a storage calculation unit for storing and calculating calculation programs and various data, fetching teaching data, It consists of an interface for outputting computed values. Here, the reproduction operation of the taught data (hereinafter referred to as work data) will be briefly described below.
[0053]
The control program is stored in the ROM, and the trajectory planning unit 22 in the program reads the work data stored in the hard disk 15 as shown in FIG. 1, and the work trajectory of the tool or workpiece gripped by the robot tip is tertiary. Planned on the Cartesian coordinates of the original space.
[0054]
Next, the interpolation point calculation unit 16 divides the work trajectory for each interpolation period of a predetermined time, and calculates the position / orientation that the tool or the like should reach for each interpolation period as interpolation point data. Then, the inverse transform unit 17 inversely transforms the position / orientation represented by the orthogonal coordinates in the interpolation point data into the position at each joint level necessary for reproduction by the articulated robot, and each servo driver 2 1 , 2 2 , 2 Three The position command value for is calculated.
[0055]
The position command to the servo driver is temporarily stored in the RAM, and a synchronization signal S is sent from the data converter 18 that converts the joint position of the robot into a servo position command value. YNC Is transmitted to each servo driver in synchronization with The above processing is the most basic processing for controlling the articulated robot by the teaching playback method, and it goes without saying that it is well known.
[0056]
Incidentally, the ROM shown in FIG. 3 stores a program for executing robot operation control, collision detection and stop processing, and data such as execution conditions and control constants. The RAM is used as a working area for the CPU, and temporarily stores data being calculated. The hard disk 15 stores not only work data but also data unique to the robot, such as link length and mass, and data related to workpieces and tools that are unique in the taught operation.
[0057]
A serial I / O (input / output) 19 is connected to the teach pendant 14 so that the CPU can recognize work data obtained by operating the teach pendant through serial communication. Servo IF (interface) 20 is servo driver 2 1 , 2 2 , 2 Three To enable transmission of command data and monitoring of motor control status. The timer 21 generates a synchronization signal S at regular intervals. YNC This signal gives the update timing of the command value to the servo driver.
[0058]
Next, the first embodiment for performing the collision detection of the robot and the stop process after the collision according to the present invention will be described with reference to FIG. The stop process will be described later, and will be described from collision detection. The basic point of collision detection is the comparator 29. 1 , 29 2 , 29 Three The point of comparison in is the torque, not the position deviation.
[0059]
By the way, when trying to detect the presence or absence of a collision based on torque, it must be noted that even if a large torque is applied, it cannot be said that the collision is unconditionally performed. When the joint moves with a large acceleration, a large torque is required, so it cannot be considered a collision just because the torque suddenly increases. Therefore, it is impossible to detect a collision with high accuracy unless it is determined exactly how much torque is necessary to make the desired motion and how far it is from the torque that is currently being produced. The present invention has been completed through research from this viewpoint.
[0060]
First, since the servo driver not only controls the drive of the motor but also can monitor the state of the servo motor, in this example, motor information such as the position / speed / acceleration of the motor is extracted from the servo driver. Then, this is input to the collision detection unit 11 of the controller 13, and H (θ) · θ described in detail later. dd + C (θ ・ θ d ) + G (θ) · g Performs inverse dynamics calculation based on the formula, and torque T that should be generated by each joint drive motor if there is no collision 1 , T 2 , T Three Predict.
[0061]
Inverse dynamics calculation is to calculate the torque generated at each joint of the robot from the position / velocity / acceleration state of each joint of the robot and the mechanism parameters of the robot. According to this inverse dynamics calculation, the influence of the torque fluctuation factors (1) to (4) described above can be accurately predicted.
[0062]
On the other hand, the “actually generated torque” to be compared with the predicted torque is a torque value picked up before the torque loop of the servo driver. If there is no collision, it is almost the same as the torque value calculated by inverse dynamics, and if the difference between the two torques is large, it means that there is a collision. Therefore, a “threshold value” ε described later in which the difference is predetermined for each joint i If it has reached, it will be regarded as “collision”.
[0063]
T i = H (θ) · θ dd + C (θ ・ θ d ) + G (θ) · g requires data specific to the robot, that is, mechanism parameters, which are read from the hard disk. The torque value may be either a current value or N · m (Newton meter). If there is a torque conversion unit having a torque constant (N · m / A) in the servo driver, it can be output in N · m, and this can be matched with the calculated value. Needless to say, if the servo driver does not have a torque conversion unit, it may be converted into a current value and compared in reverse dynamics calculation.
[0064]
A servo IF 20 is installed between each servo driver and the robot controller, and the following data is transmitted. As shown in FIG. 1, from the servo driver 2 to the controller 13, the motor position response θm [rad], the motor speed response θ d m [rad / sec], motor acceleration response θ dd m [rad / sec 2 ] And a torque command value Tm [N · m] to the motor, and from the controller to the servo driver, a position command value θ [rad], a position feedback proportional gain Kpp and a torque limit value Tmdec [N · m] described later. is there.
[0065]
For the detection of the collision, as long as the feedback control of the servo driver is performed, the five steps shown in FIG. 4 are sequentially repeated. First, in step 1 (hereinafter referred to as S1 in the drawing), the collision detection unit 11 receives a response value and a torque command of the position / speed / acceleration of the motor driving each joint of the robot from the servo driver 2 via the servo IF 20. Read the value. This reading is performed for all the motors that drive the joints. In the case of a robot consisting of three joints, the motor position / velocity / acceleration response values and torque command values are expressed by equations (1-1) to (1-4) using the symbols in Table 1, respectively. It becomes a matrix. The number attached to the symbol in the formula represents the joint number, [] T Indicates that the matrix described in the “number” column of Table 1 is “transposed”.
[Table 1]
[Equation 5]
[0066]
Incidentally, although the number of joints is 3 in the embodiment, it can be applied with n ≧ 2 for the purpose of the present invention. In the case of n = 1, it is not theoretically impossible, but a low-degree-of-freedom robot usually does not have a high speed and does not receive interference from other joints, so there is no need for matrix calculation. Note that the idea of the present invention is applied to each robot even in a system in which, for example, two robots having a tool mounted on one side and a workpiece gripped on the other side are operated in conjunction with each other. is there.
[0067]
By the way, the motor and the robot joint are generally coupled via a speed reducer. Therefore, it is necessary to convert the values of equations (1-1) to (1-4) into the position, velocity, and acceleration at the robot joint. The response values of position / velocity / acceleration at the joint and the driving torque can be expressed by equations (1-5) to (1-8) using the symbols in Table 2, respectively.
[Table 2]
[Formula 6]
[0068]
Taking the i-th joint as an example, they are expressed by equations (1-10) to (1-13). For the position, speed, and acceleration, the reduction ratio ζ i The value obtained by dividing by is the joint conversion value, and the value obtained by multiplying the reduction ratio in the torque is the joint conversion value. Therefore, in step 2, the position / velocity / acceleration of the motor level is changed to the value of the joint level of the robot.
[Expression 7]
[0069]
If the joint level position / velocity / acceleration and torque response values are known, in Step 3, they will be generated by the inverse dynamics calculation applied to the inverse dynamics model (H, C, G) described below. Torque is calculated. The inverse dynamic model includes an H matrix of inertia matrix, a C matrix of viscosity matrix, and a G matrix of gravity matrix. The physical meaning of each matrix will be described later.
[0070]
From the mechanism parameters of the robot (center of gravity, mass, inertia tensor, length, etc. of the link connected to the joint) and the position / velocity / acceleration of each joint, the torque T generated at each joint is expressed by equation (2-1) It is known that it can be calculated by This is called an inverse dynamics model.
[Equation 8]
[0071]
Equation (2-1) is described in Shigeo Hirose “Robot Engineering-Vector Analysis of Mechanical Systems”, Hankabo, 2nd edition, published on February 10, 1989 (hereinafter referred to as Technical Document 1). . In section 11.3 [4], the procedure for performing inverse dynamics calculation using the Newton-Euler method, which is one of the rigid body motion analysis techniques, is described in detail. Ultimately, the inverse dynamics calculation formula is the formula (11.28) in Technical Document 1, but the last term J in the formula T K (multiplying the transposed Jacobian matrix by the external force applied to the tip of the robot and the K representing the moment due to the external force) is the case where the robot is not colliding in the present invention, that is, without touching anything in the space. It is not necessary to take into consideration since the torque at the time of displacement is to be obtained. Therefore, the above equation (2-1) can be obtained by omitting the term.
[0072]
In the case of a robot with n joints, the inertia matrix H is an n × n regular symmetric matrix, which means torque generated in the joint itself due to acceleration of each joint of the robot and interference torque generated due to acceleration of other joints. The viscosity matrix C is an n × 1 matrix, and shows the torque generated by the influence of centrifugal force and Coriolis force generated by the speed of each joint of the robot. The gravity matrix G is an n × 3 matrix, and the torque generated by the influence of gravity acceleration Is shown.
[0073]
I will touch on the inertia matrix H in more detail. The diagonal element Hjj of the inertia matrix H (see equation (2-9) described later) represents the moment of inertia indicating the relationship of the torque that the acceleration of the jth joint exerts on the jth joint itself, that is, Hjj is The load inertia varies depending on the joint position. The other component, Hij, is the moment of inertia representing the relationship of the interference torque exerted on the i-th joint by the acceleration of the j-th joint.
[0074]
The inertia matrix H, viscosity matrix C, and gravity matrix G can be calculated from the mechanism parameters of the robot and the position / velocity of each joint. The mechanism parameters of the robot are composed of the following four. In addition, the number in Table 3 is per link connected to the joint. Incidentally, the third link L in the case of the robot shown in FIG. Three If a tool such as a welding torch is attached, the parameter of the third link is given in a state where the tool is integrated.
[Table 3]
[0075]
The robot mechanism parameters such as the mass, center of gravity, inertia tensor, and length of each link are known values that can be obtained in advance from the robot mechanism, link shape, and material. Since it is clear that it does not change, it is a fixed value and is stored in the hard disk 15 in advance. If these mechanism parameters are read out and applied to equation (2-1), the torque generated in each joint can be calculated from the current position / velocity / acceleration of each joint and the gravitational acceleration g. Each element of formula (2-1) has the meaning described in Table 4.
[Table 4]
[0076]
Here, a description will be given of the mechanism parameters of the robot in this example. Mass m i , Center of gravity mx i , My i , Mz i , Inertia tensor Ix i Or Iz i Represents the mass of the link connected to the i-th joint that is moved by the motion of the i-th joint and not moved by the motion of the i + 1-th joint, the position of the center of gravity of the link, and the inertia tensor of the link. Since the calculation procedure of the inertia tensor is described in detail in Section 3.8 of the technical document 1 (see equation (3.38)), the description is omitted here.
[0077]
The reference for calculating the position of the center of gravity and the inertia tensor is placed in the link coordinate system. The procedure for defining the link coordinate system assigned to each joint is explained in detail in Section 2.9 of Tsuneo Yoshikawa's “Robot Manipulator” Corona, first edition issued on December 15, 1989 (hereinafter referred to as Technical Document 2). Here, the definition of the basic link coordinate system will be described with reference to FIG. For example, the second joint 7 2 And the third joint 7 Three Link L across 2 , The link coordinates assigned to the second joint are determined with the rotation center of the second joint as the origin, and the x direction is defined as the link L 2 The z direction is determined as the rotation direction of the second joint, and the y direction is determined as the direction of the right-handed coordinate system.
[0078]
Next, the inertia tensor I is generally used to indicate the difficulty of rotation of the object, and is expressed by the equation (2-2) which is a well-known 3 × 3 symmetric matrix. The diagonal component represents the moment of inertia of each joint of the robot mechanism, and the other components represent the negative value of the product of inertia.
[Equation 9]
[0079]
The moment of inertia is obtained by the following equation, and represents the difficulty around the object (robot link) and the difficulty of stopping.
[Expression 10]
[0080]
Next, the inertial product is obtained from the following equation, and represents the moment that disturbs the movement of the object (robot link) when the object is rotating.
[Expression 11]
Here, dm indicates the mass of a minute volume. Further, x, y, z in the formula is expressed with reference to a predetermined link coordinate system, and the link L shown in FIG. 2 The reference coordinates of the inertia tensor of the link coordinate system assigned to the second joint is the link L 2 The coordinate system when moved to the center of gravity position. The same applies to other joints.
[0081]
The calculation method of the matrix values of inertia, viscosity, and gravity is described in detail in Technical Document 1. Although not described here, in the case of a robot composed of three joints, the matrix of inertia, viscosity, and gravity is expressed as follows.
[Expression 12]
[0082]
Each component of Equation (2-9) to Equation (2-11) is composed of the parameters shown in Table 3, joint position θ, joint velocity θ d Consisting of only θ, θ d All other values are known values. Therefore, the joint position response θr and the joint speed response θ obtained in step 2 d r is θ, θ as d , The inertia matrix H, viscosity matrix C, and gravity matrix G in equation (2-1) can be obtained.
[Formula 13]
[0083]
As the matrixes of H, C, and G are calculated in step 3, the unknown values in equation (2-1) are the joint acceleration θ dd In addition, there are two gravitational acceleration vectors g indicating the direction in which the gravitational acceleration acts. The latter changes depending on the installation posture of the robot, but in general, it can be considered that the installation posture of the robot does not change, and can be defined in advance.
[0084]
In step 4, an unknown value necessary for calculating the inverse dynamics model finally expressed by equation (2-1) is the joint acceleration θ dd It becomes only. This is also the joint acceleration response θ obtained in step 2 dd By substituting r in the form of equation (4-1), each joint of the robot is θr, θ when there is no collision. d r, θ dd The torque T generated at each joint required to exercise at r can be predicted by equation (4-2).
[Expression 14]
[0085]
When the collision calculated in step 4 does not occur, each joint of the robot is θr, θ d r, θ dd In step 5, a difference ΔT between the torque T of each joint necessary to move at r and the torque command value Tc that the motor actually tries to drive the joint calculated in step 2 is calculated.
[Expression 15]
[0086]
If there is no collision, the torque command value Tc and the predicted value T are almost equal and should be ΔT≈0. On the other hand, the absolute value of ΔT is a predetermined determination value ε i If it becomes larger than that, it is assumed that a collision has occurred, and the control program is shifted to the stop process. Specifically, if a condition that satisfies any one of the conditional expressions (5-3) to (5-5) occurs, it is determined that a collision has occurred.
[Expression 16]
[Table 5]
[0087]
By the way, ε i Is, for example, 0.1 Tc i Often it is sufficient. Actually 0.3Tc i However, if a torque of 30% or more is applied at that time, it is determined as a collision. In the case of the positional deviation described in the section of the prior art, if converted into torque, for example, 0.8 Tc. i It can be seen that the collision detection accuracy is remarkably improved in the case of the present invention as compared with the case where it is more than that. Such an improvement can be expected to further improve depending on the joint.
[0088]
In addition, judgment value ε i May be handled as simple as fixed, but it can be changed according to the magnitude of acceleration and speed, and the sensitivity can be changed by decreasing the sensitivity if acceleration or speed is large and increasing it if small. It can also be left. Increasing the sensitivity at the time of acceleration / deceleration and setting it to 0.1 Tc tends to cause erroneous detection of a collision, and if the sensitivity is decreased to 0.3 Tc when acceleration / deceleration is low and the speed is low, detection of the collision is delayed. Because it will be.
[0089]
As can be seen from the above description, in order to detect the collision of the robot, the information of each motor that drives all the joints is processed in an integrated manner, and all the interference is calculated by integrating them and calculating the inverse dynamics. (Collisions) can be found. Needless to say, equation (2-1) is indispensable for the calculation. Among the mechanism parameters of the robot, the link mass, link barycentric position, and link length are indispensable elements. However, although the link inertia tensor is necessary for a robot employing a long and narrow link, it may not be considered in the case of a lightweight robot or a short link robot, and may be appropriately selected each time.
[0090]
Servo driver 2 1 , 2 2 , 2 Three The “position command” input to is a command issued as a result calculated by the trajectory planning unit 22 based on the data stored in the hard disk 15 (see FIG. 1). 14 (see FIG. 3), this is a “position command” based on the data input. Therefore, the operation of the present invention can be performed even for an unexpected collision during teaching.
[0091]
According to the collision detection method of the present invention, collision detection is performed with high accuracy and speed. Therefore, in combination with a stop processing operation described later, at least the robot breaks or damages mechanical components such as jigs existing in the vicinity. And the time required for restoration is reduced as much as possible, and the operation and teaching after the collision can be resumed promptly. Since collision detection is performed by the collision detection software installed in the controller, it is not necessary to equip the robot link with a shock sensor or mechanical collision detection device, avoiding the complexity of the structure and mechanism of the robot link and reducing the weight. .
[0092]
In FIG. 1, it has been described that the motor position response, motor speed response, motor acceleration response, and torque command value to the motor are transmitted from the servo driver to the controller. However, since the speed response and the acceleration response are obtained by differentiating the former, the servo driver to the controller is limited to the motor position response and speed response as shown in FIG. The acceleration response of the motor may be obtained by differentiating the speed response value. Furthermore, although only the motor position response is supplied to the controller, although not shown, it is also possible to obtain the speed and acceleration by first-order and second-order differentiation by the controller. In either case, it can be expected that the congestion of information transmission via the servo IF 20 is reduced.
[0093]
In the above example, when the difference ΔT is obtained, the estimated torque value based on the inverse dynamics calculation and the torque command value Tm generated by the servo driver through speed control are treated as the actual joint drive torque, and the torque command value is adopted. explained. However, instead of the torque command value, a torque calculated from a torque response value in each motor may be adopted.
[0094]
As can be seen from FIG. 7, the servo driver 2 1 , 2 2 , 2 Three Torque command value Tm obtained by i DC motor 1 with each brush from the power line according to the power 1 , 1 2 , 1 Three Servo AMP2a supplied to 1 , 2a 2 , 2a Three The circuit from the current detector 23 1 , 23 2 , 23 Three Is provided. Then, after A / D converting the current picked up by the current detector, its torque value Tn i Multiplied by the torque constant, and the torque response value (actually output torque) Tc obtained as N · m i Use '. In this case, the following equation is adopted instead of the above equation (1-13).
[Expression 17]
[0095]
When the motor is a brushless DC motor, not all the current actually flowing becomes an effective torque. In particular, when the motor rotates at high speed, the reactive current that is not converted into torque increases, and the error between the command and the output increases. The torque actually output by the motor is detected by the current flowing in two of the three phases of the motor (hereinafter referred to as phase current), and the coordinate conversion unit 24 shown in FIG. 8 performs coordinate conversion. Thus, the effective current i which is the current converted into torque qa Can be requested.
[0096]
The coordinate transformation process is to discard the reactive current and extract only the effective amount. About this, Hidehiko Sugimoto et al., “Theory and Design of AC Servo System”, Sogo Denshi Publishing, 2nd edition, May 1990 This is described in detail in section 4.2 of the daily issue (hereinafter referred to as Technical Document 3). Finally, it is represented by the formula (4.11) in this document. I in the formula qa Is equivalent to the effective current, and the phase current i ua , I va And motor electrical angle θ re More effective current i qa Is calculated by equation (6-1).
[Expression 18]
[0097]
The electrical angle θ re Although there are many methods for detecting this, when an absolute encoder is used, it can be obtained as follows. First, the pulse displacement θe of the encoder corresponding to an electrical angle of 360 ° rev Is calculated. θe rev Is the pulse displacement θm per encoder revolution rev Is divided by the number of pole pairs Pn of the motor, and is calculated by equation (6-2). The number of pole pairs is a number in which three pole pairs u, v, and w of a brushless DC motor are arranged per one rotation of the motor, and the rotation amount of the motor corresponding to one pole pair is an electrical angle of 360 °. Become. Pulse displacement θm per encoder rotation rev The number of pole pairs Pn of the motor is also a value that can be defined in advance from the specifications of the encoder and the motor.
[Equation 19]
[0098]
An encoder pulse displacement θe corresponding to an electrical angle of 360 ° is output pulse θe from the encoder. rev The remainder at that time is θe rev The electrical angle θ is obtained by multiplying the value divided by 2π by 2π. re Is required. Specifically, the equation (6-3) is obtained. The operator MOD in the equation represents calculating the remainder of division.
[Expression 20]
[0099]
The active current i calculated from the above equations (6-1) to (6-3) qa Is multiplied by the torque constant Kt to calculate the torque Tn that the motor is actually outputting. The torque constant Kt is a ratio of the generated torque Tn to the effective torque, and is a value that can be defined in advance from the specifications of the motor.
[Expression 21]
[0100]
The symbols shown above are listed in Table 6 for reference. FIG. 8 described above is a block diagram of a servo driver having a function of detecting output torque. u , 23 v Phase current i detected in ua , I va Is read through the AD converter. The encoder output pulse required for calculating the electrical angle can also be read.
[Table 6]
[0101]
If the motor is a brushed DC motor, the coordinate conversion in FIG. 8 is not necessary, and the torque constant may be immediately multiplied. If the output torque of the motor is detected and used for collision detection in this way, the detection accuracy is further improved. In particular, the reactive current that is not converted to torque during high-speed rotation increases, and this is an extremely effective means in a brushless DC motor or the like that tends to cause a deviation between the torque command value and the motor output torque.
[0102]
In FIG. 1, the inverse dynamics calculation is performed using the position / velocity / acceleration response value picked up from the servo driver. coming out. FIG. 9 is intended to improve this. Work data as position / velocity / acceleration of the motor used when predicting the torque to be generated by the motor that drives each joint when no collision has occurred. The joint position calculated for the reproduction operation and the velocity and acceleration calculated based on the joint position are employed.
[0103]
The position, speed, and acceleration of the motor should be known at the stage of trajectory planning, and if this is used, the predicted torque can be calculated without looking at the servo status. Is based. Therefore, the torque expected to be generated in each motor is obtained from the position, velocity, and acceleration obtained as a result of analyzing the trajectory plan by inverse dynamics calculation, and it is matched with the torque actually flowing in the motor to detect the presence of collision. I will try.
[0104]
Therefore, in addition to the trajectory planning unit 22, the interpolation point calculation unit 16, and the inverse conversion unit 17 shown in FIG. 1, the control program in the controller 13 of FIG. A speed / acceleration calculation unit 25 is added to calculate the speed / acceleration.
[0105]
In addition, it is necessary to take time adjustment between the output of the position command signal from the data conversion unit 18 to the servo driver 2 and the output of the data signal 26 for inverse dynamics calculation. For example, it may be considered that the operation is delayed from the former by, for example, the calculation time of the servo driver. With the configuration as shown in FIG. 9, the amount of data exchanged between the servo driver 2 and the controller 13 is reduced, and the communication load is reduced. Incidentally, even during teaching with the teach pendant or manual operation, the trajectory planning unit 22 can generate the work trajectory and predict the joint position, velocity, and acceleration. As in the case of the reference), the present invention can be applied to other than the operation playback.
[0106]
The collision detection is realized by sequentially repeating the six steps shown in FIG. 10 as long as the feedback control of the servo driver 2 is performed on the interpolation point data of the work trajectory of the robot. Note that the collision detection is performed by the collision detection unit 11 which is a program in the controller 13, which is not different from the example of FIG. 1.
[0107]
First, in step 11, the position / velocity / acceleration of each joint of the robot is calculated in the process of reproducing the work data stored in the hard disk 15. Specifically, the velocity / acceleration calculation unit 25 calculates the velocity θ of each joint of the robot from each joint position θ of the robot for each interpolation period. d , Acceleration θ dd Is calculated.
[0108]
Here, a method for calculating the speed and acceleration of each joint of the robot will be described. When the position θ of each joint of the robot for each interpolation cycle Smpt is calculated, the joint position calculated in the current cycle is θcurr, the joint position in the previous interpolation is θprev, and the velocity θ of each joint at the current interpolation point d Is calculated as follows.
[Expression 22]
[0109]
The joint speed calculated in the current cycle is θ d Let curr be the joint velocity at the time of the previous interpolation θ d If prev, acceleration θ of each joint at the current interpolation point dd Is required in the same way. Note that the speed and acceleration of each joint at time 0 are both set to 0.
[Expression 23]
[0110]
From the position θ of each joint of the robot calculated in step 11, a position command θc to the servo driver is calculated by the equation (8-1) and is output to the servo driver 2 via the servo IF 20. Since the motor drives the joint via the speed reducer, a value obtained by multiplying the position θ of each joint of the robot by the reduction ratio becomes a command value to the servo driver. In this way, the position command value is transmitted to the servo driver in step 12. Θc is the position response [rad] of the robot joint, and the number thereof is n (= 3) × 1 matrix.
[Expression 24]
[0111]
In step 13, the torque state is read. That is, the collision detection unit 11 receives the torque command value Tm of the motor that drives each joint of the robot from the servo driver 2 via the servo IF. 1 , Tm 2 , Tm Three Is read. In addition, since the coupling from the motor to the robot joint is performed via a reduction gear, the torque command value Tm in the servo driver is used. i The value obtained by multiplying the reduction ratio by the torque Tc that drives the joint i As given.
[Expression 25]
[0112]
Thereafter, the inverse dynamics model (H, C, G) is calculated in step 14, and the predicted torque value by the inverse dynamics calculation is calculated in step 15. In step 16, the collision determination process is performed by the procedure as described above. Of course, the stop process described later is also performed thereafter.
[0113]
In the example of FIG. 9, the torque is predicted based on the position / velocity / acceleration command, and it is clear that the accuracy of the torque prediction increases if the error between the actual response value and the command value is small. Therefore, the collision detection accuracy is also improved. Therefore, in the configuration of FIG. 9, if the speed command is feedforward compensated for the servo control system in the servo driver 2, the command tracking performance of the servo driver can be improved, and the robot collision detection accuracy is always kept high. It will be possible to keep. FIG. 11 shows a mode in which feedforward compensation of speed command is added to the configuration of FIG.
[0114]
More specifically, in the case of FIG. 9, since the joint speed is also calculated after the trajectory plan, the speed can be fed forward from the data converter 27 as shown in FIG. If feedforward is performed, the degree of the relationship between the position, velocity, and acceleration differing from the actual response can be made smaller than in the case of FIG. That is, in the case of FIG. 9, the position / velocity / acceleration tends to deviate from the actually used position / velocity / acceleration. There arises a problem in calculation accuracy such that no match occurs. However, this can be effectively solved by feedforward compensation.
[0115]
FIG. 12 shows how the speed response differs depending on the presence or absence of feed-forward compensation of the speed command. If feedforward compensation of the speed command is performed, the tracking performance (speed response) is improved as indicated by the solid line with respect to the speed command indicated by the broken line, as can be seen by comparing (a) and (b). As can be seen, the feedforward reduces the time lag, and as a result, the relationship between position, speed, and acceleration is very close to reality.
[0116]
Note that when the feedforward compensation is performed, the effectiveness of the position feedback proportional gain Kpp is reduced, but since the speed is effective in a feedforward manner, the speed command is made quickly. However, since sufficient position accuracy cannot be obtained only by speed, speed control based on the position remains, and this is made to function in an interpolating manner for speed feedforward.
[0117]
By the way, formula (2-1) is J T The reason why K is omitted is that it is intended to be applied to, for example, an arc welding robot that always works without touching anything outside and displaces the space. Therefore, when the present invention is applied to a type of robot that works while touching a workpiece, it is effective only when the tool is simply spatially moved from the current position to the next target position. However, the equation (2-1) T If K is added, the inverse dynamics calculation is accompanied by a slight increase in processing time. However, the idea of the present invention can be applied even when a force sensor or the like is in contact with the workpiece. Can be reflected.
[0118]
Although the collision detection method has been described so far, the stop process after the collision is detected will be described. The purpose of the processing is as follows. Needless to say, when a collision occurs, it should be detected as soon as possible. However, since a little time has already passed since the collision was detected, it is extremely difficult to stop the robot just before the collision.
[0119]
Therefore, in the operation of stopping the robot, the ultimate goal should be to minimize damage to the robot main body, the object to be hit, and the obstacle even if the robot collides. Simply put, each motor should be able to generate only enough torque to absorb and stop the moment of inertia that the link that is being moved through its joints now has, so that A stop process is performed from the viewpoint of enabling escape operation and minimizing damage.
[0120]
As described in FIG. 2, this stop process is a control that allows rotation in a direction that softens the interference force caused by the collision as shown by an arrow 9 for a joint that is stopped or moving at a low speed, As described in the section of the prior art, when trying to brake quickly by generating as much torque as possible at any joint, a large force acts between the robot and the obstacle. It is something to try to avoid.
[0121]
Specifically, after the collision is detected, the generated torque of the motor is limited so as not to generate a torque exceeding the minimum torque necessary for setting each motor speed to zero. For example, if the third joint is stationary and gravity is not acting, the torque may be almost zero, so if it collides, the joint rotates easily and the link escapes from the obstacle. If it is possible to escape without load, the second and third joints have room for coasting to stop, reducing the interference and impact forces of the entire robot and minimizing the occurrence of damage. . As described above, the present invention intends to propose a mechanism for escaping a joint that does not have a large inertial force or does not generate a high speed without opposing the force.
[0122]
The main purpose of the stop process is to stop the joint after it has collided with the joint that is stopped or operating at low speed, or for a joint whose inertia force is sufficiently small compared to the maximum torque. It is that the interference force caused by the collision is eased by letting the object escape.
[0123]
In addition, the minimum torque value required to stop the robot operation is calculated for each joint from the speed and load inertia of each joint at the time of the collision, and the minimum torque value is a torque limiter in the servo driver. The position feedback proportional gain Kpp in all servo drivers is replaced with 0. As a result, the deceleration process is forcibly performed by speed feedback control in which the speed command value is set to zero.
[0124]
Furthermore, in calculating the torque value of each joint required to stop the robot operation, it is assumed that all joints stop at the same time, and after calculating the deceleration stop time for each joint, all deceleration stop times By allocating the maximum value among these to the deceleration stop time of any joint, the above-described escape operation can be realized in some joints.
[0125]
Even if a collision is detected, the speed does not become zero in steps, so the joint speed θ when the collision occurs d r is the motor speed θ which is a state variable of the servo driver d m. The joint position can also be known from the motor position θm which is a state variable of the servo driver. If the joint position at the time of occurrence of the collision is known, it is possible to calculate the H matrix that is the load inertia. This is an inertia that naturally changes depending on the posture of the robot. From this inertia and speed at the time of the collision occurrence, a torque necessary to reduce the speed to 0 is calculated, and the motor output torque is limited so that torque exceeding this value is not generated. This torque output restriction process is performed via the torque restriction value transmission path 28 shown in FIGS.
[0126]
The third joint 7 as shown in FIG. Three The first joint 7 is not moving 1 And second joint 7 2 Is moving, the first joint and the second joint require a relatively large torque to control the inertial force, but the third joint has a small inertial force, so the torque required for deceleration is also the first. Compared to one joint or the second joint, it may be small. Therefore, the third joint 7 Three A small value is set in the torque limiter of the motor for driving the third joint, and even if the link connected to the third joint collides with the movement of the first joint and the second joint, the third joint can easily interfere with the interference. It is possible to escape in the direction that softens, and can absorb changes due to collision.
[0127]
A specific example of the stop process will be described below. After detecting a collision by any of the collision detection processes described above, a process consisting of the four steps of FIG. 13 is performed. First, in step 21, a torque value that can be used to stop the robot operation is calculated.
[0128]
The torque value Tusab of each joint that can be used for stop processing when a collision occurs can be calculated from Equation (10-1) obtained by transforming Equation (4-2). This torque value Tusab can be used for a deceleration motion for stopping the operation, and is regarded as a torque generated by acceleration. The torque due to gravity and the torque due to viscosity depend on the speed, and must be secured separately from the torque due to acceleration.
[0129]
If the torque (viscosity torque) that disappears due to the speed by transforming the equation (4-2) and the torque that disappears due to gravity are removed, it becomes the torque that can be used for deceleration. That is, the torque that can actually be used for the deceleration motion for stopping is limited by the disturbance torque that affects the centrifugal force and the gravitational acceleration.
[Equation 26]
[0130]
In addition, in formula (10-1), sign (θ d r) determines the sign. Tmax is the maximum torque that can be generated by each motor, and is set to a positive value. However, in some cases you may want to use it as a negative value. d r) determines this. If the current speed is positive, negative acceleration is required and negative torque is applied. When the speed is positive, Tmax is positive, but since it has a negative sign, the torque is negative. Since a positive acceleration is required if a negative speed is output, negative x negative = positive is set. That is, the speed (θ d This means that the sign is inverted with respect to r).
[0131]
−sign (θ d r) · Tmax is the maximum deceleration torque that can be output by each joint. The deceleration torque required to reduce the current speed to 0 is the subtraction of the torque lost due to speed and the torque lost due to gravity from that torque, and it can be used for actual deceleration ( The torque Tusab given in 10-1).
[0132]
Equation (10-1) is equivalent to H (θr) · θ in Equation (4-2). dd r is Tusab and T in equation (4-2) is -sign (θ d r) is replaced by Tmax. The maximum output torque value Tmax of the i-th joint i Can be calculated by equation (10-4) in consideration of the reduction ratio, where Tm max is the maximum torque that can be output by the motor that drives the joint. Needless to say, Tm max is a value that can be defined in advance from the specifications of the motor, and the viscosity matrix C and the gravity matrix G can be calculated from the position and speed of each joint when a collision occurs.
[Expression 27]
[0133]
Tm max i Also the reduction ratio ζ i Is also a known value, so Tmax i Can also be fixedly provided in advance. The C matrix and G matrix are also calculated in the process of detecting a collision, and do not need to be newly calculated. Since the current one is acceptable, it is sufficient to bring the one used in Equation (4-2) as it is. Actually, there is a slight time lag, but it is not possible to calculate again from the time of collision. Of course, it may be calculated, but it takes time, and the calculated value does not differ greatly. The symbols appearing above are listed in Table 7 for reference.
[Table 7]
[0134]
In step 22, the torque value of each joint necessary for stopping the operation is calculated. The torque generated by deceleration is the product of the inertia matrix H and the deceleration of each joint, and is expressed by equation (11-1). Θ dd Is unknown, but if it is known, Tdec can be obtained by this equation.
[Expression 28]
[0135]
By the way, in the control of the stop process, it is assumed that all the joints decelerate and stop at a speed of 0 at time tmd. Then, the deceleration θ of each joint dd Can be expressed by equation (11-2). Note that θ in equation (11-2) d r is each joint speed at the time of collision occurrence. However, tmd is unknown at this stage.
[Expression 29]
[0136]
The reason that the control condition is that all joints decelerate and stop at time tmd in equation (11-2) is that control cannot be executed unless some condition is imposed. Thus, the above-described escape operation can be performed at the joint.
[0137]
Therefore, as the time tmd, the longest time tmd is used from a plurality of tmds determined by the number of robot joints in the expressions (11-4) to (11-6) described later (expression (11)). (See 11-7)). This is to prevent the torque generated by the deceleration motion of the robot after the collision at all the joints of the robot from exceeding the torque Tusab that can be used for the deceleration. Can be stopped at the same time.
[0138]
For reference, the selected tmd joint is often the fastest moving joint or the joint that receives the largest moment. By the way, according to the time selected in this way, the slowest moving joint has enough time until the speed becomes 0, and unnecessary coasting is forced, but the speed is originally low. So even if you run a little, the distance will not increase.
[0139]
Here, since the torque that can be used for deceleration of the stop process is calculated as Tusab from Expression (10-1), the right side of Expression (11-1) is replaced with Tusab, and all joints are expressed by Expression (11-2 ) To decelerate and stop at time tmd. Θ in formula (11-1) dd Substituting the right side of equation (11-2) into, equation (11-1) is transformed into equation (11-3). The calculation method of the inertia matrix H in the equation (11-3) is as already described.
[30]
[0140]
From this equation (11-3), the deceleration stop time tmd at which the speed of each joint of the robot can be zero can be calculated within the range of the torque Tusab that can be used for deceleration of the stop process. This is because in equation (11-3), H (θr) can be calculated from each joint position θr at the time of the collision, and θr can be monitored via the servo IF 20. And θ d r is each joint velocity at the time of collision occurrence, and can be monitored via the servo IF, and Tusab is already calculated by the equation (10-1).
[0141]
Eventually the only unknown value is tmd. tmd is a determinant. In the case of a robot having three joints, three relational expressions can be obtained. Actually, the equation (11-3) is decomposed into the following three equations for each joint, and the deceleration stop time tmd is calculated. Specifically, since the number of joints n = 3 in this embodiment, the deceleration stop time tmd for each joint. 1 Or tmd Three After calculating tmd 1 , Tmd 2 , Tmd Three The maximum value in is used for the deceleration stop time of all joints. MAX in the equation (11-7) means that the maximum value in the element is extracted.
[31]
[0142]
Torque value Tdec of each joint necessary to stop operation i Is calculated by Expression (11-8) using the deceleration stop time tmd calculated from Expression (11-4) to Expression (11-7). Tdec i Is a torque value [N · m] of joints necessary for stopping, and the number thereof is n (= 3) × 1 matrix. tmd is the deceleration stop time [sec] at all joints, and the number is 1.
[Expression 32]
[0143]
Tdec calculated in this way takes into account load inertia and fluctuations in disturbance torque that change depending on the position and speed of each joint of the robot at the time of collision, and is the minimum torque that does not cause a delay in the stop time. Value.
[0144]
H matrix in equation (11-1), current joint velocity θ in equation (11-2) d From r and the deceleration stop time tmd, the deceleration torque Tdec required for each joint can be obtained from equation (11-8). One joint, that is, a joint that seems to require the maximum time for the deceleration operation, is equal to Tusab, but other joints have Tdec <Tusab. This is because Tdec is also small when the speed is low or the inertial force (= H) is small. When this is set as the torque limit value, the escape operation can be performed with respect to the interference force.
[0145]
In step 23, the set value of the torque limiter 3 in the servo driver 2 is changed. That is, Tdec calculated in step 2 is a torque generated at the joint, and the servo driver performs the process of limiting the torque actually generated by the motor. Usually, since the motor drives the joint via the speed reducer, Tdec is changed to the converted value Tm dec of the motor shaft, and Tm dec is transmitted to the torque limiter 3 of each servo driver through the transmission path 28 via the servo IF 20. Set to
[0146]
The minimum necessary torque value Tm deci that does not cause a delay in the stop time of the motor that drives the i-th joint can be calculated as shown in Expression (12-1). Tm dec is a torque value [N · m] of the motor necessary for stopping, and the number thereof is n (= 3) × 1 matrix.
[Expression 33]
[0147]
By the way, Tdec in equation (11-8) enters the servo driver as the torque limit value, but Tdec cannot be entered as it is. Since the dynamic calculation is generally obtained at the joint level, the obtained Tdec is also the joint level torque, and it is the motor that actually limits the torque. Since a reduction gear is interposed between the motor and the joint, a value divided by the reduction ratio must be set. So actually Tm dec i Will be put in.
[0148]
As a reminder, H matrix and θ d Since r is different for each joint, tmd 1 = Tmd 2 = Tmd Three Tm dec i Is servo driver 2 1 , 2 2 , 2 Three Different for each. The obtained Tmdec i May be used as is, but Tmdec i However, it may be convenient to weight each joint. In such cases, a different coefficient is assigned to each Tmdec. i You can apply by multiplying.
[0149]
In step 24, the position feedback proportional gain Kpp in each servo driver 2 is changed to 0 via the servo IF 20. Through the processing in steps 21 to 24 described above, deceleration processing is performed by speed feedback control in which the speed command value is forcibly set to 0.
[0150]
The torque limiter 3 in each servo driver is set with a minimum torque value necessary for stopping. Therefore, when the interference force due to the collision exceeds this value, the force that the joint interferes (collision force) is released. Operates and reduces impact. Compared to the conventional case in which the maximum torque value is set in the torque limiter, the interference force mitigating effect is drastically improved.
[0151]
In this way, Kpp is set to 0 as well as replacing the torque limit value because the joint cannot be stopped only by replacing the torque limit value. Regardless of the value of position command or position feedback, if Kpp is set to 0, the speed command is immediately set to 0. This is because it is easy to set Kpp to 0 in order to simply set the speed command to 0.
[0152]
As can be seen from the above, the movement of the joint is stopped only by the speed loop, but since the torque is appropriately changed and limited, control is performed to stop at the maximum torque in the fast moving joint. . On the other hand, in other joints, it is possible to generate only a torque that keeps its own speed at 0, that is, keeps the current posture.
[0153]
As can be seen from the above, according to the stopping process described above, when an interference force or impact exceeding the torque limit value is applied, the joint can be made to perform force-relief behavior using the superiority or inferiority of the force. The occurrence of damage due to this is suppressed as much as possible. Even in a stopped state, the integral term in the speed loop will continue to function whether the joint hits an obstacle or not, so each link maintains its posture with the torque within the limit value exerted by the joint. Keep doing.
[0154]
By the way, for a link that only moves in the horizontal plane and does not require force against gravity to maintain posture, or a link that is in an unfavorable situation when the torque is applied, the speed becomes zero. To release torque. In that case, the function of the integral term containing Kvi may be eliminated, and an operation for clearing the integral term may be added when Kpp is set to 0 or thereafter.
[0155]
With the above, the description of the stop process after the collision detection is finished, and the measures for improving the collision detection accuracy will be described below. As already mentioned, the torque response value can be used instead of the torque command value, but here the inertia moment of the motor rotor is taken into account, and the frictional resistance of the motor bearing and reducer is taken into account. The improvement of the collision detection accuracy by touching is described. Any of them can function alone or in duplicate.
[0156]
First, the description will be given from the improvement of the collision detection accuracy by considering the moment of inertia of the motor rotor. This is because the “inertia moment indicating the relationship of the torque that the acceleration of the joint exerts on the joint itself” as a diagonal element in the n × n inertia matrix, that is, “the load inertia that varies depending on the joint position”, The inertia moment of the motor rotor including the speed reducer is to be multiplied by the square of the speed reduction ratio and added. Thus, the collision detection accuracy can be improved in consideration of the torque generated by the moment of inertia of the motor rotor including the speed reducer in each joint.
[0157]
First, the torque generated by the moment of inertia of the motor rotor including the speed reducer in each joint is taken into consideration by modifying the inertia matrix H of equation (2-9) as follows. Here, Im is the moment of inertia of the motor rotor [kg · m 2 ], The number is n (= 3) × 1 matrix.
[Expression 34]
[0158]
As is common to the examples of FIG. 1, FIG. 9, and FIG. 11, the inertia matrix H only calculates the inertia of the link connected to each joint. The joint moves because it is driven by a motor, but the motor is also driven before the joint is driven, and attention should be paid to the rotational inertia of the rotor itself at that time.
[0159]
Where Im i Is the moment of inertia of the motor rotor including the speed reducer. The moment of inertia of the rotating body that moves in conjunction is multiplied by the square of its reduction ratio to affect the moment of inertia of the input shaft. Therefore, the influence of the moment of inertia of the input shaft on the output shaft is the opposite, and must be multiplied by the square of the reduction ratio. This is why the moment of inertia of the motor rotor including the speed reducer is multiplied by the square of the speed reduction ratio and added.
[0160]
The diagonal element of the inertia matrix H indicates the influence of torque applied to itself, and the other elements indicate the influence of interference torque applied to other joints. Since the moment of inertia of the motor rotor including the speed reducer does not interfere with other joints, only the diagonal elements need be corrected. Im i Ξ i Since it is already known that can be set in advance, collision detection and stop processing can be performed in the same way as the already described example even in the case of Expression (13-1).
[0161]
Next, increasing the accuracy of collision detection in consideration of frictional resistance will be described. By changing the robot dynamics model expressed by Equation (2-1) as shown in Equation (14-1), it will be possible to consider the frictional resistance of motor bearings and reducers in each joint. Is. The symbols appearing here are listed in Table 8 for reference.
[Expression 35]
[Table 8]
[0162]
The reducer itself has a lot of friction, and this cannot be ignored. The velocity term is called the viscosity matrix, but it is Cm · θ d It is. However, this is not intended for friction when objects are rubbed. When an object moving at a certain speed overlaps an object moving at a certain speed, the speed and speed interfere with each other to generate torque in another direction, which means that torque. This is expressed in the four items of the equation (14-1).
[0163]
Cm is a kinematic viscosity coefficient, and if this is multiplied by speed, it becomes disturbance torque. | Fcou | is called Coulomb friction (static friction: resistance until the movement starts), and has a certain magnitude without depending on the speed, but is a torque component that always works in the direction opposite to the motion. Cm ・ θ d The sum of m and | Fcou | is the friction torque to be considered here. Equation (14-1) is obtained by adding four items to Equation (2-1), and is intended to increase the accuracy by calculation considering friction.
[0164]
Here, Cm is a dynamic viscosity coefficient n × 1 matrix of a motor bearing or a speed reducer, and the dynamic viscosity coefficient is a constant representing a proportional relationship between speed and dynamic friction torque. The dynamic friction torque is a torque component having a magnitude proportional to the speed. Furthermore, θ d m is a motor speed n × 1 matrix. Note that sgn (θ in equation (14-1) d m) is a sign function determined as follows based on the speed of the motor.
[Expression 36]
[0165]
Further, the relationship between the joint speed and the motor speed is expressed by Expression (14-5) from the reduction ratio ξ.
[Expression 37]
Here, ξ is a reduction ratio of the reduction gear in the joint. Since the parameters Cm, Fcou and ξ are also known values that can be set in advance, it goes without saying that collision detection and stop processing can also be performed by the above (14-1).
[0166]
【The invention's effect】
According to the present invention, when a collision does not occur from the motor position, speed, and acceleration detected by the servo driver, the torque to be generated by each motor is predicted, and the predicted torque value and the actual joint driving torque are calculated. Since the difference is calculated and the absolute value is larger than the preset judgment value, it is assumed that a collision has occurred, so that any link or tool of the robot is subject to an object such as a workpiece or other obstacle A collision with an object can be detected quickly and accurately through torque.
[0167]
The torque that should be generated by the motor that drives each joint when there is no collision is the position θ and speed θ of each joint of the robot. d , Acceleration θ dd And T = H (θ) · θ based on the robot's mechanism parameters dd + C (θ, θ d ) + G (θ) · g is predicted by the inverse dynamics calculation, so that the predicted torque value can be obtained with high accuracy, without installing a mechanical component such as a collision sensor. Can detect collisions quickly and accurately.
[0168]
Among joints, joints that are stopped or operating at low speed or joints that require a sufficiently small torque compared to the maximum torque because the inertial force is small are allowed to escape after a collision. Thus, it is possible to reduce the interference force caused by the collision and suppress the occurrence of property damage.
[0169]
The minimum torque value required to stop the robot operation is calculated for each joint from the speed and load inertia of each joint when a collision occurs, and the minimum torque value is set in the torque limiter in the servo driver. At the same time, if the position feedback proportional gain Kpp in all servo drivers is set to 0 and the deceleration process is executed by speed feedback control with the speed command value set to 0, any value can be used for the position command and position feedback. The speed command can be simply set to 0 even if the collision occurs. In addition, when the interference force due to the collision exceeds the torque limit value, the joint moves in a direction to release the interference force and the collision force is reduced. .
[0170]
In calculating the torque value of each joint required to stop the robot operation, it is assumed that all joints stop at the same time, and after calculating the deceleration stop time for each joint, all the deceleration stop times are calculated. If the maximum value is used for the deceleration stop time of any joint, there will be room for escape operation even if a collision occurs, and the damage caused by the collision will be absorbed to minimize the occurrence of damage. Can do.
[0171]
As the position, speed, and acceleration of the motor used for predicting torque, the joint position calculated in the reproduction operation of work data to give a position command to the servo driver, and the speed and acceleration calculated based on the joint position If this is applied, the collision detection calculation can be performed quickly only for the time to acquire the information of the position / velocity / acceleration of the motor from the servo driver.
[0172]
In inverse dynamics calculation, an n × n inertia matrix indicating the torque generated at each joint by the acceleration of each joint of the robot and the interference torque generated by the acceleration of other joints, and n indicating the influence of centrifugal force and Coriolis force Since the x1 viscosity matrix and the nx3 gravity matrix indicating the torque generated by the influence of gravitational acceleration are included, the calculation accuracy is extremely high.
[0173]
The inertia moment of the motor rotor including the speed reducer is multiplied by the square of the reduction ratio to “the moment of inertia indicating the relationship between the acceleration of the joint exerted on the joint itself” as a diagonal element in the n × n inertia matrix. Thus, collision detection can be performed in consideration of the torque generated by the moment of inertia of the motor rotor including the speed reducer in each joint.
[0174]
If the dynamic viscosity coefficient of the speed reducer and the Coulomb friction torque are reflected in the formula used for the inverse dynamics calculation, it is highly accurate considering the frictional resistance of the motor bearings and speed reducers in each joint. Collision detection is realized.
[0175]
If the center of gravity, mass, and length of the link connected to the joint are given as the mechanism parameters of the robot used for inverse dynamics calculation, the calculation will be in accordance with each robot, and even the inertia tensor If added, more accurate torque prediction becomes possible.
[0176]
If the torque calculated from the torque command value actually generated by the servo driver is used as the actual joint drive torque from which the difference from the predicted torque value is obtained, the calculation speed can be kept high. When the torque calculated from the torque response value in each motor is used, the accuracy for collision detection is further improved in comparison with the actual torque.
[Brief description of the drawings]
FIG. 1 is a block diagram showing an entire robot control system to which a collision detection / stop control method is applied in an articulated robot according to the present invention.
FIG. 2 is a schematic diagram of a robot representing a simplified structure with three joints and a escaping action when a link hits an obstacle.
FIG. 3 is a control block diagram showing an outline of signal transmission between a robot controller (control device), a teach pendant, and a joint driving motor.
FIG. 4 is a flowchart showing a processing procedure for determining the presence or absence of a collision.
FIG. 5 is an explanatory diagram of a second link giving a reference coordinate of inertia tensor.
FIG. 6 is a block diagram showing the entire control system including an example of changing a response value transmitted from a servo driver to a controller.
FIG. 7 is a block diagram of a servo driver when a torque response value is adopted instead of a torque command value.
FIG. 8 is a block diagram of a servo driver when using a brushless DC motor.
FIG. 9 is a block diagram showing an entire robot control system in which inverse dynamics calculation is performed based on speed and acceleration obtained from joint positions generated by trajectory planning.
FIG. 10 is a flowchart when the processing procedure based on FIG. 9 is adopted in determining the presence or absence of a collision.
FIG. 11 is a block diagram of the entire robot control system when the speed is fed forward to the servo driver.
12A and 12B show the degree of delay in the speed response to the speed command, where FIG. 12A is a response diagram when the speed is not feed forward, and FIG. 12B is a response diagram when the feed forward is performed.
FIG. 13 is a flowchart showing a procedure for stop processing.
FIG. 14 is a schematic configuration diagram of a servo driver in which various control loops for driving a motor are incorporated.
FIG. 15 is a block diagram of the prior art showing that a position deviation is extracted from a servo driver to determine the presence or absence of a collision, and a stop process can be performed subsequently.
[Explanation of symbols]
1,1 1 , 1 2 , 1 Three ... Motor, 2, 2 1 , 2 2 , 2 Three ... servo driver, 3 ... torque limiter, 7 1 ... first joint, 7 2 ... second joint, 7 Three 3rd joint, 8 ... Obstacle, 10 ... Robot, 11 ... Collision detection unit, 12 ... Stop processing unit, 13 ... Controller, 15 ... Hard disk, 22 ... Trajectory planning unit, 23 ... Current detector, 25 ... Speed Acceleration calculator, 29 1 , 29 2 , 29 Three ... Comparator, L 1 ... 1st link, L 2 ... second link, L Three ... 3rd link, θm 1 , Θm 2 , Θm Three ... Motor position detected by servo driver, θ d m 1 , Θ d m 2 , Θ d m Three ... Motor speed detected by servo driver, θ dd m 1 , Θ dd m 2 , Θ dd m Three ... Motor acceleration detected by the servo driver, T 1 , T 2 , T Three ... Predicted torque value generated by each motor when no collision occurs, Tm 1 , Tm 2 , Tm Three ... Torque command value actually generated by servo driver, ΔT 1 , ΔT 2 , ΔT Three ... Difference between torque predicted value and torque calculated from torque command value, ε 1 , Ε 2 , Ε Three ... collision judgment value, Kpp ... position feedback proportional gain.

Claims (11)

複数の関節からなるロボットのリンクやリンク先端に把持されたツールまたはワークが周辺の障害物等に衝突したとき、リンク等の変形やロボット可動部の損傷を回避すべく衝突したことを検出しかつ直ちにロボットを停止させる方法において、
ロボットの各関節駆動用モータを制御するためのサーボドライバが検出したモータの位置・速度・加速度から、衝突が発生していない場合に各モータが発生すべきトルクを、ロボットの各関節の位置θ、速度θd 、加速度θddとロボット機構パラメータとをもとにした
で示す式に基づく逆動力学演算により予測し、
該トルク予測値と実際の関節駆動トルクとの差分を求め、
該差分の絶対値が予め設定された判定値より大きい場合に衝突が発生したものと見做し、
複数の関節のうち停止もしくは低速で動作している関節あるいは慣性力が小さいため動作停止に必要なトルクが最大トルクと比較して十分小さい関節は、衝突があった後に衝突による干渉力を和らげるべく逃げ動作できるようにしたことを特徴とする多関節ロボットにおける衝突検出・停止制御法。
When a robot link consisting of multiple joints or a tool or work gripped at the tip of the link collides with an obstacle around it, it detects that it collided to avoid deformation of the link or damage to the robot moving part, and In the method of stopping the robot immediately,
Based on the position, speed, and acceleration of the motor detected by the servo driver for controlling each joint drive motor of the robot, the torque that should be generated by each motor when there is no collision is calculated as the position θ of each joint of the robot. , Based on speed θ d , acceleration θ dd and robot mechanism parameters
Predicted by inverse dynamics calculation based on the formula shown in
Find the difference between the predicted torque value and the actual joint drive torque,
When the absolute value of the difference is larger than a preset determination value, it is assumed that a collision has occurred,
Among joints that are stopped or operating at low speed, or because the inertial force is small, the joint that is sufficiently small in torque compared to the maximum torque should reduce the interference force due to the collision after a collision A collision detection / stop control method for articulated robots, characterized by being able to escape.
衝突発生時の各関節の速度と負荷イナーシャからロボットの動作を停止させるために必要な最小限のトルク値を関節ごとに算出し、その最小限のトルク値をサーボドライバのトルク制限器に設定すると共に全てのサーボドライバの位置フィードバック比例ゲインを0とし、強制的に速度指令値を0とした速度フィードバック制御による減速処理を行わせるようにしたことを特徴とする請求項1に記載された多関節ロボットにおける衝突検出・停止制御法。  Calculate the minimum torque value required for stopping the robot operation for each joint from the speed and load inertia of each joint when a collision occurs, and set the minimum torque value in the torque limiter of the servo driver. The multi-joint according to claim 1, wherein the position feedback proportional gain of all servo drivers is set to 0 and the speed reduction control is forcibly performed with the speed command value set to 0. A collision detection / stop control method for robots. ロボットの動作を停止させるために必要な各関節のトルク値の算出においては、全ての関節が同じ時間で停止すると仮定し、関節ごとに減速停止時間を算出した後、全ての減速停止時間のうちの最大値をいずれの関節の減速停止時間にも充てるようにしたことを特徴とする請求項1または請求項2に記載された多関節ロボットにおける衝突検出・停止制御法。  In calculating the torque value of each joint required to stop the robot operation, it is assumed that all joints stop at the same time, and after calculating the deceleration stop time for each joint, The collision detection / stop control method for a multi-joint robot according to claim 1 or 2, wherein the maximum value of the above is applied to the deceleration stop time of any joint. 衝突が発生していない場合に各関節を駆動するモータが発生すべきトルクを予測するときに使用されるモータの位置・速度・加速度は、サーボドライバが検出したモータの位置・速度・加速度に代えて、サーボドライバに位置指令すべく予め教示されている作業データの再生動作により演算した関節位置と該関節位置をもとにして計算された速度および加速度であることを特徴とする請求項1ないし請求項3のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。  The motor position, speed, and acceleration used to predict the torque that should be generated by the motor that drives each joint when no collision has occurred are replaced with the motor position, speed, and acceleration detected by the servo driver. 2. A joint position calculated by a reproduction operation of work data taught in advance to give a position command to a servo driver, and a speed and an acceleration calculated based on the joint position. The collision detection / stop control method for an articulated robot according to claim 3. 前記逆動力学演算は、ロボットの関節数をnとして、ロボットの各関節の加速度により各関節で発生するトルクならびに他の関節の加速度により発生する干渉トルクを示すn×nの慣性行列、遠心力とコリオリ力の影響を示すn×1の粘性行列、および重力加速度の影響により発生するトルクを示すn×3の重力行列を含むことを特徴とする請求項1ないし請求項4のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。  The inverse dynamics calculation is an n × n inertia matrix indicating the torque generated at each joint by the acceleration of each joint of the robot and the interference torque generated by the acceleration of another joint, centrifugal force, where n is the number of joints of the robot 5. An n × 1 viscosity matrix indicating an influence of Coriolis force and an n × 3 gravity matrix indicating a torque generated by the influence of gravitational acceleration are included. Collision detection / stop control method for articulated robots described in 1. 前記n×nの慣性行列における対角要素としての「関節の持つ加速度が該関節自体に及ぼすトルクの関係を示した慣性モーメント」には、減速機を含むモータロータの慣性モーメントが減速比の2乗倍して加算され、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮することができるようにしたことを特徴とする請求項5に記載された多関節ロボットにおける衝突検出・停止制御法。  In the “inertia moment indicating the relation of the torque that the acceleration of the joint exerts on the joint itself” as a diagonal element in the n × n inertia matrix, the inertia moment of the motor rotor including the reduction gear is the square of the reduction ratio. 6. The collision detection / multi-joint robot according to claim 5, wherein the torque generated by the moment of inertia of the motor rotor including the speed reducer in each joint can be taken into account. Stop control method. 前記逆動力学演算に供される式に、
ただし、Cmi 関節iにおける減速機の動粘性係数
(i=1,2,・・・n 以下同じ)
θdi 関節iにおけるモータの速度応答
Fcou i 関節iにおける減速機のクーロン摩擦トルク
sgn (θdi ) 符号関数
= 1 ; θd m>0のとき
= 0 ; θd m=0のとき
=−1 ; θd m<0のとき
が加算され、各関節内のモータ軸受や減速機の摩擦抵抗を考慮することができるようにしたことを特徴とする請求項1ないし請求項6のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。
In the formula used for the inverse dynamics calculation,
However, the kinematic viscosity coefficient of the reducer at Cm i joint i
(Same for i = 1, 2,... N)
θ d m i Motor speed response at joint i
Coulomb friction torque of reducer at Fcou i joint i
sgn (θ d m i ) sign function
= 1; when θ d m> 0
= 0; when θ d m = 0
Any one of claims 1 to 6, wherein the case of θ dm <0 is added, and the frictional resistance of the motor bearing and the speed reducer in each joint can be taken into consideration. A collision detection / stop control method for an articulated robot according to claim 1.
前記ロボットの機構パラメータは、各関節に連結されるリンクの重心位置・質量・長さであることを特徴とする請求項1ないし請求項7のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。  8. The collision in the multi-joint robot according to claim 1, wherein the mechanism parameter of the robot is a position of a center of gravity, a mass, and a length of a link connected to each joint. Detection / stop control method. 前記ロボットの機構パラメータには、慣性テンソルも付加されていることを特徴とする請求項8に記載された多関節ロボットにおける衝突検出・停止制御法。  9. The collision detection / stop control method for an articulated robot according to claim 8, wherein an inertia tensor is also added to the mechanism parameter of the robot. 前記実際の関節駆動トルクは、実際にサーボドライバが生成したトルク指令値から算出されるトルクであることを特徴とする請求項1ないし請求項9のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。  10. The articulated robot according to claim 1, wherein the actual joint driving torque is a torque calculated from a torque command value actually generated by a servo driver. Collision detection / stop control method. 前記実際の関節駆動トルクは、各モータにおけるトルク応答値から算出されるトルクであることを特徴とする請求項1ないし請求項9のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。  The collision detection / stop in the multi-joint robot according to any one of claims 1 to 9, wherein the actual joint driving torque is a torque calculated from a torque response value in each motor. Control method.
JP2001081553A 2001-03-21 2001-03-21 Collision detection / stop control method for articulated robots Expired - Fee Related JP4756618B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2001081553A JP4756618B2 (en) 2001-03-21 2001-03-21 Collision detection / stop control method for articulated robots

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2001081553A JP4756618B2 (en) 2001-03-21 2001-03-21 Collision detection / stop control method for articulated robots

Publications (2)

Publication Number Publication Date
JP2002283276A JP2002283276A (en) 2002-10-03
JP4756618B2 true JP4756618B2 (en) 2011-08-24

Family

ID=18937634

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2001081553A Expired - Fee Related JP4756618B2 (en) 2001-03-21 2001-03-21 Collision detection / stop control method for articulated robots

Country Status (1)

Country Link
JP (1) JP4756618B2 (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3366107A1 (en) 2017-02-23 2018-08-29 Panasonic Corporation Harvesting equipment and harvest method
KR20200071588A (en) * 2018-12-11 2020-06-19 주식회사 코아로봇 Direct teaching method of controlling robot and direct teaching robot system
SE544423C2 (en) * 2020-04-06 2022-05-17 Husqvarna Ab A robotic work tool system and method with collision-based command interface

Families Citing this family (49)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004201485A (en) 2002-11-29 2004-07-15 Sony Corp Motor and controller of actuator
JP2004181600A (en) * 2002-12-05 2004-07-02 Sony Corp Leg type moving robot
JP2004181601A (en) * 2002-12-05 2004-07-02 Sony Corp Leg type moving robot
JP2004181599A (en) * 2002-12-05 2004-07-02 Sony Corp Leg type moving robot
US7366587B2 (en) 2002-12-05 2008-04-29 Sony Corporation Legged mobile robot
JP4086765B2 (en) * 2003-11-28 2008-05-14 ソニー株式会社 Robot equipment
JP4115374B2 (en) * 2003-11-18 2008-07-09 ソニー株式会社 Robot device
JP2004195554A (en) * 2002-12-16 2004-07-15 Sony Corp Leg type mobile robot
JP2004249374A (en) * 2003-02-18 2004-09-09 Sony Corp Leg type mobile robot
JP2004364396A (en) * 2003-06-04 2004-12-24 Yaskawa Electric Corp Controller and control method for motor
EP2380710B1 (en) 2003-07-29 2012-11-28 Panasonic Corporation Robot arm control method and control device
CN100413657C (en) * 2003-07-29 2008-08-27 松下电器产业株式会社 Robot arm control method and control device
JP4223911B2 (en) * 2003-09-25 2009-02-12 株式会社神戸製鋼所 Collision detection method and collision detection apparatus
JP4258718B2 (en) * 2003-09-29 2009-04-30 株式会社安川電機 Robot controller
JP4440072B2 (en) * 2004-10-26 2010-03-24 パナソニック株式会社 Robot control method
JP2006341271A (en) * 2005-06-08 2006-12-21 Honda Motor Co Ltd Method and apparatus for monitoring truing operation of electrode tip of spot welding robot
JP4668251B2 (en) * 2007-10-05 2011-04-13 株式会社神戸製鋼所 Robot system control device, control method and program thereof
JP5689590B2 (en) * 2009-05-18 2015-03-25 Juki株式会社 sewing machine
JP5383911B2 (en) * 2010-06-22 2014-01-08 株式会社東芝 Robot controller
US8483877B2 (en) * 2010-09-03 2013-07-09 GM Global Technology Operations LLC Workspace safe operation of a force- or impedance-controlled robot
US9119655B2 (en) 2012-08-03 2015-09-01 Stryker Corporation Surgical manipulator capable of controlling a surgical instrument in multiple modes
ITMI20111868A1 (en) 2011-10-13 2013-04-14 Gima Spa PROCEDURE FOR CONTROL OF INTERFERENCE AND / OR COLLISION OF MECHANICAL ORGANS AND CONNECTED SYSTEM
US9226796B2 (en) 2012-08-03 2016-01-05 Stryker Corporation Method for detecting a disturbance as an energy applicator of a surgical instrument traverses a cutting path
CA2879414A1 (en) * 2012-08-03 2014-02-06 Stryker Corporation Systems and methods for robotic surgery
FR3002048B1 (en) * 2013-02-14 2016-07-01 Commissariat Energie Atomique METHOD FOR ENHANCED COLLISION COLLECTION OF A ROBOT WITH ITS ENVIRONMENT, SYSTEM AND COMPUTER PROGRAM PRODUCT IMPLEMENTING THE METHOD
JP5633623B2 (en) * 2013-12-11 2014-12-03 株式会社デンソーウェーブ Robot system
ES2648295T3 (en) * 2014-11-07 2017-12-29 Comau S.P.A. Industrial robot and control procedure of an industrial robot
EP3351356B1 (en) 2015-09-16 2020-11-18 Panasonic Intellectual Property Management Co., Ltd. Robot collision detection method
CN108136604A (en) * 2015-10-30 2018-06-08 川崎重工业株式会社 The monitoring arrangement of robot system
EP3373837B8 (en) * 2015-11-11 2024-01-24 MAKO Surgical Corp. Robotic system and method for backdriving the same
KR101724490B1 (en) 2015-12-02 2017-04-07 기아자동차 주식회사 Vehicle production system using cooperattion robot
KR101734241B1 (en) 2015-12-10 2017-05-11 현대자동차 주식회사 Trunk lid hinge intellectual loader unit
SG11201910185TA (en) * 2017-05-29 2020-01-30 Franka Emika Gmbh Collision handling by a robot
JP6804479B2 (en) * 2018-01-15 2020-12-23 Dmg森精機株式会社 Motor drive control device and machine tools equipped with it
CN108247654A (en) * 2018-02-05 2018-07-06 遨博(北京)智能科技有限公司 A kind of control method of robot, device and system
JP7211007B2 (en) 2018-10-30 2023-01-24 セイコーエプソン株式会社 Control device, robot system and control method
US10369701B1 (en) * 2018-10-30 2019-08-06 Mujin, Inc. Automated package registration systems, devices, and methods
CN113165181A (en) * 2019-01-17 2021-07-23 西门子股份公司 Robot collision detection system and method and robot comprising system
DE102019108390B3 (en) * 2019-04-01 2020-08-06 Franka Emika Gmbh Specifying safe speeds for a robot manipulator
CN111941403B (en) * 2019-05-17 2023-07-04 本田技研工业株式会社 Link mechanism, control device and control method thereof
JP7422215B2 (en) * 2020-03-16 2024-01-25 株式会社Fuji articulated robot
WO2021192181A1 (en) 2020-03-26 2021-09-30 三菱電機株式会社 Friction compensation device, collision sensing device, torque feedforward computation device, robot control device, and friction compensation method
CN114012734B (en) * 2021-12-03 2023-05-30 西安交通大学 Parameter-adaptive robot collision detection method
CN114260900B (en) * 2021-12-30 2023-08-29 库卡机器人制造(上海)有限公司 Deceleration control method and device for robot, robot assembly and storage medium
CN114516052B (en) * 2022-03-23 2023-12-22 杭州湖西云百生科技有限公司 Dynamics control method and system for parallel real-time high-performance multi-axis mechanical arm
CN114734437B (en) * 2022-03-25 2024-09-20 中国科学院自动化研究所 Robot joint control method and device
CN114800519B (en) * 2022-05-20 2023-09-26 天津大学 Six-degree-of-freedom industrial robot dynamic parameter identification method considering friction
CN115229849B (en) * 2022-08-29 2024-06-14 哈尔滨博实自动化股份有限公司 Vibration suppression method for joint type heavy load out-of-furnace robot under strong impact load
CN115674190B (en) * 2022-09-30 2024-05-07 深圳市越疆科技股份有限公司 Cooperative mechanical arm and motion control method, collision detection method and control system thereof

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04160605A (en) * 1990-10-25 1992-06-03 Murata Mach Ltd Collision preventing device for automatic operating machine tool
JP2749724B2 (en) * 1991-01-16 1998-05-13 ファナック株式会社 Collision detection method by estimated disturbance
JP3459973B2 (en) * 1999-10-22 2003-10-27 川崎重工業株式会社 Drive control method and drive control device

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3366107A1 (en) 2017-02-23 2018-08-29 Panasonic Corporation Harvesting equipment and harvest method
JP2018134059A (en) * 2017-02-23 2018-08-30 パナソニック株式会社 Harvesting device and harvesting method
KR20200071588A (en) * 2018-12-11 2020-06-19 주식회사 코아로봇 Direct teaching method of controlling robot and direct teaching robot system
KR102130620B1 (en) * 2018-12-11 2020-07-06 주식회사 코아로봇 Direct teaching method of controlling robot and direct teaching robot system
SE544423C2 (en) * 2020-04-06 2022-05-17 Husqvarna Ab A robotic work tool system and method with collision-based command interface

Also Published As

Publication number Publication date
JP2002283276A (en) 2002-10-03

Similar Documents

Publication Publication Date Title
JP4756618B2 (en) Collision detection / stop control method for articulated robots
JP6924146B2 (en) Robot system monitoring device
US8583285B2 (en) Method and device for stopping a manipulator
JP6238021B2 (en) ROBOT, ROBOT CONTROL DEVICE AND CONTROL METHOD, AND ROBOT CONTROL PROGRAM
US4621332A (en) Method and apparatus for controlling a robot utilizing force, position, velocity, spring constant, mass coefficient, and viscosity coefficient
JP4683073B2 (en) Robot grip control device and robot grip control method
JP5327722B2 (en) Robot load estimation apparatus and load estimation method
CN110494260B (en) Apparatus and method for controlling cooperative robot
CN101332604A (en) Control method of man machine interaction mechanical arm
JP6664138B2 (en) Control method and robot device
US8483877B2 (en) Workspace safe operation of a force- or impedance-controlled robot
JP2009285824A (en) Device and method for controlling robot manipulator
WO2003068464A1 (en) Drive control method and drive controller
JP2020101541A (en) Method and device for torque estimation
JP3367641B2 (en) Robot control device
Lo et al. Cooperative teleoperation of a multirobot system with force reflection via internet
JP2004364396A (en) Controller and control method for motor
Je et al. Current based compliance control method for minimizing an impact force at collision of service robot arm
JPH1170490A (en) Collision detecting method for industrial robot
JP2006116650A (en) Collision detecting method of robot
JP5904445B2 (en) Robot controller
JP2003245881A (en) Apparatus and method for controlling robot
Mohammad et al. Towards human-robot collaboration with parallel robots by kinetostatic analysis, impedance control and contact detection
Hu et al. Position and force control of flexible joint robots during constrained motion tasks
Nahavandi et al. Automated robotic grinding by low-powered manipulator

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20080311

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20110208

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20110331

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20110517

A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20110527

R150 Certificate of patent or registration of utility model

Ref document number: 4756618

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

Free format text: JAPANESE INTERMEDIATE CODE: R150

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20140610

Year of fee payment: 3

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

LAPS Cancellation because of no payment of annual fees