JP4756618B2 - 多関節ロボットにおける衝突検出・停止制御法 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は多関節ロボットにおける衝突検出・停止制御法に係り、詳しくは、ロボットが稼働中にワーク等の外界の対象物や障害物等に衝突すれば、それをいち早くかつ精度よく検出し、その後速やかにロボットの動きを止めたり力を解放して、ロボット本体やツールの変形，損傷を回避できるようにしたロボットの制御法に関するものである。
【０００２】
【従来の技術】
複数の関節からなるロボットのリンクやロボットの先端に把持されたツールもしくはワークが、教示データに基づいて再生動作しているときやティーチペンダントによる教示操作で動いているとき、意図することなく加工対象物や周辺の障害物に当たることがある。再生動作中であれば、ロボットの各関節を駆動するそれぞれのモータは指令値の通りに、また教示中でも衝突に気づかず教示を続ければ、ロボットは当然に障害物等を突破してでも動こうとする。その結果、関節によっては過大なトルクが発生し、それに伴ってリンクやツールが変形したり関節等のロボットの可動部を損傷させるといった事態を招く。
【０００３】
ロボットに教示された作業データの再生動作を実行させる各モータを制御するためのそれぞれのサーボドライバには、例えば図１４に示すように、位置や速度の制御系が組み込まれ、図示しない制御装置（ロボットコントローラ）から与えられる位置指令にモータ１の動作を追従制御させ、作業データの再生動作が実行されるようにしている。また、各サーボドライバ２には速度制御系の後にトルク制限器３が配置され、速度制御によって生成されるトルク指令を任意の値で制限しておくことができるようにもなっている。
【０００４】
この制御系から分かるように、一番外側のループである位置フィードバック系のエラー成分、すなわち位置指令から位置フィードバック値を差し引いた大きさ（位置の偏差）を監視すれば、衝突の有無を検出することができる。その最も基本的なものは、図１５に示すように、位置制御ループにおける加算器４の直後からモータ１の位置をフィードバック制御した結果得られる位置偏差を取り出し、比較器５で「位置偏差のしきい値」である各関節における判定値６と比較して、大きければ衝突が発生したと判定するというものである。
【０００５】
このような制御によれば、ショックセンサトーチがぶつかるとスイッチが入りサーボ電源が落ちる仕組みになっていたり、機械式の衝突検出機構が装備されている検出装置など従来から存在するものに比べれば、ロボットとして必要最小限の構成要素であるモータさえ備えることで、すなわち衝突センサを有しなくてもロボットコントローラに衝突検出ソフトを搭載するだけで衝突を検出することが可能となる。従って、ロボット本体の機構の複雑化をきたさないようにしておくことができる。
【０００６】
衝突を検出する別の方法として、特許第２，７４９，７２４号に記載された外乱推定オブザーバを用いた衝突検出方法がある。これは、各サーボドライバの中に外乱トルクを観測するオブザーバを備え、実際にモータが回そうとしているイナーシャはどれくらいの大きさのものであるかを内部に持つモデルによって推定すると共に衝突により生じた外乱トルクを検出し、その外乱トルクの推定値に対する大小を見て衝突の有無を判定しようとするものである。
【０００７】
すなわち、内部でモデル化することにより或る指令値に対してどのような挙動をするかを予測し、その予測挙動と実際の挙動とを突き合わせることによって先読み補償するという思想である。従って、イナーシャがモデル化できていればトルク予測は可能であり、イナーシャに変動がなければ高い精度で衝突を検出することができるはずである。複数の関節を備えたモデル化はともかくとして、単一の関節だけで扱うとすればイナーシャのモデル化は容易であるからである。
【０００８】
このように、衝突の検出はぶつかったことによるサーボの状態変数の変化があって始めて可能となり、各モータ単位で検出されるトルクの変動によって衝突を感知しているのである。
【０００９】
ところで、衝突を検出した後にロボットの動きを止める必要があるが、その停止のためにはいずれの関節においても可能なかぎりの大きさのトルクを発生させれば迅速に制動できるとの考えから、トルク制限器には最大トルク値が設定されるのが一般的である。最大トルクとはそれぞれのモータにおいて発生することができる最大のトルクを意味していることは言うまでもない。
【００１０】
具体的には、位置フィードバックゲインを０とすることで速度制御系への指令を強制的に０とし、速度０を目標値とした速度フィードバック制御を行うことにより停止処理を行う。停止のための減速に使用するトルクは、速度制御系の後に配置されたトルク制限器により常にモータの最大許容トルクまで可能となっている。
【００１１】
【発明が解決しようとする課題】
ところが、前記した図１５の衝突検出システムにおいては、いずれかの関節の位置のエラー成分が十分に蓄積されなければ、衝突を検出したと認識することができない。それがために、衝突を検出した時点では既にモータが大きなトルクを発生した状態となっており、ロボット本体やツールの変形・損傷を可及的に少なくすることはできない。すなわち、衝突を検出することはできても、その時点で手遅れの状態になっているのである。
【００１２】
例示すれば、図２に示した第３関節７₃ が第３リンクＬ₃ を破線のごとく真っ直ぐな姿勢に保持しようとしていても、例えば第２関節７₂ によって押されれば障害物８に当たる。第３リンクＬ₃ は障害物８によって実線のように動かされようとするがフィードバック制御が効いているので、第３関節Ｌ₃ を駆動しているモータ１₃ が直ちに最大トルクで真っ直ぐな位置へ戻そうとする。当然に干渉力は増大し、大きな力が相互に作用することになってしまう。
【００１３】
図１５にも表されているように、サーボドライバー２にはトルク制御、その外側に速度制御、最外側に位置制御があるので、衝突に対して一番鋭敏なのはトルクとなる。その次に速度であり、一番鈍感なのが位置である。それゆえ、位置偏差で衝突を検出しようとすると、以下に述べる欠点は覆いようがない。
【００１４】
フィードバック制御においては速度やトルクを発生させるために位置偏差が常に存在しており、それが高速でしかも大きな加速度で動作するとき大きくなることは言うまでもない。一方、ゆっくりと動作する場合には、障害物に当たったときでも位置偏差が小さい。
【００１５】
そこで、小さな位置偏差で衝突であると判定させるようにしておくと、速い速度，大きい加速度で動作するときには小さな位置偏差を直ちに越えてしまうので衝突を誤検出することが多く、ロボットは止まってばかりで使い物にならなくなる。逆に、大きな位置偏差で衝突であると判定させるようにしておくと、遅い速度，小さな加速度で動くときには十分に行き過ぎなければ衝突を検出できなくなる。
【００１６】
これから分かるように、通常考えられる一番厳しい動かし方をしても衝突と誤認識しないだけの大きな設定値を入れざるを得なくなり、位置偏差をもとにした衝突の検出は鈍感なものとなってしまう。
【００１７】
特許第２，７４９，７２４号のように外乱推定オブザーバを使用した場合、そのオブザーバは一般的には速度ループに組み込まれる。これはサーボドライバにおいての話であるから各関節レベルで外乱を検出することになる。単一の関節だけで扱うとすればイナーシャのモデル化は容易であるが、複数の関節を持つ場合のモデル化は極めて難しく、多関節ロボットにおいては、衝突検出精度を十分に高く確保することが困難であると言える。
【００１８】
複数の関節からなるロボットでは、その関節を駆動する任意のモータを同じ速度パターンで動作させても、モータが発生するトルクは、ロボットの姿勢によって異なるという特性を持つ。ロボットの関節位置すなわちロボットの姿勢や各関節の速度・加速度の状態により、モータが駆動すべき負荷イナーシャやモータが受ける外乱トルクが変動してしまうためである。
【００１９】
言うまでもないが、ロボットは腕を伸ばせばイナーシャが大きくなり、短くすれば小さくなる。外乱推定オブザーバによる場合は高次数の制御となるゆえ、複数の関節でオブザーバを定義することが現実には不可能に近い。オブザーバの精度に要求される項目として、遠心力やコリオリ力等によっても生じる関節間の干渉が挙げられる。一つの関節でならこれらの速度の２乗項はたいした影響を持たないが、複数の関節からなる場合その影響は大きくなる。重力加速度による外乱トルクもロボットの姿勢が変われば変化する。回転半径が変わると重力によるモーメントも変わってくる。
【００２０】
衝突の検出は、ぶつかったことによるサーボの状態変数の変化があって始めて可能となる。外乱推定オブザーバはサーボドライバに組み込まれるので外乱の検出はあくまでドライバレベルであり、従って衝突を感知するのは各モータ単位で検出されるトルクの変動であって、それは各関節レベルでの外乱検出にとどまることになる。
【００２１】
実際問題としてモータの先端にどのような制御対象物が存在するか分かっていなければならないが、一つの関節レベルで外乱トルクを検出するといってもイナーシャ変動のない場合には正常に検出できても、イナーシャ変動のある場合やイナーシャ変動の避けられない多関節ロボットにおいては、そのトルク変動があったとき何処までが起こるべくして起こったものか、それとも衝突によって起こったものかは区別をつけがたい。すなわち、複数の関節があるとき上記した負荷イナーシャの変動に加えて、次に掲げる三つの外乱トルクが存在するからであり、その判定は難しい。
【００２２】
複数の関節からなるロボットの各関節が受ける外乱トルクの種類は、以下の三つである。一つは、他の関節の加速度運動より受ける干渉トルク（ロボットの関節位置・加速度により変化する）であり、二つ目は、遠心力やコリオリ力による外乱トルク（ロボットの関節位置・速度により変化する）である。そして、三つ目は、重力加速度による外乱トルク（ロボットの関節位置により変化する）である。
【００２３】
前述したごとく、トルクは衝突の検出において最も鋭敏であるが、これを検出の対象とする外乱推定オブザーバといえども、イナーシャ変動までは把握し得ない。なぜなら関節ごとに機能するからであり、他の関節を駆動するモータの状態と連動した上記の負荷イナーシャや外乱トルクの変動を考慮することができず、衝突によって発生した外乱トルクのみを精度よく抽出することは困難となる。このような事情を踏まえると、多関節ロボットの場合に厳密さを求めるにはまだ幾つかの課題が残されていると言わざるを得ない。
【００２４】
ところで、前述したごとく、衝突後にロボットを止める際速度０を目標とした速度フィードバック制御を行うことにしているが、積分項が機能しているためにトルクは依然として発生し、それが大きくなることは避けられない。なぜなら、モータに最大トルクまで発揮させてもよいとしているのが一般的であり、モータが大きい力を発揮すれば、ロボットと障害物との間に大きな力が掛かることは必然的に起こるからである。すなわち、干渉があったときの逃げ動作が最大トルクで妨げられることになり、衝突による干渉力を全くと言ってよいほど軽減できなくなる問題がある。
【００２５】
ちなみに、衝突発生時にトルク制限器を０に設定することはできない。ぶつかっているといえども慣性力があるので、慣性力に対して制動するのは摩擦だけということになり、制動力が著しく低下する。それだけでなく、停止している関節についても、衝突による衝撃力や他の関節から受ける干渉トルクによる関節の運動を制動できなくなるからである。
【００２６】
本発明は上記した問題に鑑みなされたもので、その目的は、大きく分けて以下に記す二つの課題を克服する多関節ロボットにおける衝突検出・停止制御法を提供することである。第一の課題は、関節駆動トルクが非線形的に変動する複数の関節からなるロボットにおいても、衝突により発生する関節駆動トルクの変動から衝突が発生したことを高精度かつ短時間に検出することである。すなわち、衝突を感知しているのは各モータ単位で検出されるトルクの変動であるが、できるだけ厳密な計算をして衝突によるものか否かを見極めることができるようにしようとするのである。
【００２７】
第二の課題は、ロボットの衝突検出後の停止処理において、衝突による障害物からの干渉力を和らげ、ロボット本体やぶつかった対象物の損傷を最小限に抑えることである。言い換えれば、大きな慣性力を持っていない関節もしくは速度が発生していない関節については、力方向に逃げる仕組みを提案しようとするものである。
【００２８】
【課題を解決するための手段】
本発明は、複数の関節からなるロボットのリンクやリンク先端に把持されたツールまたはワークが周辺の障害物等に衝突したとき、リンク等の変形やロボット可動部の損傷を回避すべく衝突したことを検出しかつ直ちにロボットを停止させる方法に適用される。その特徴とするところは、図１を参照して、ロボットの各関節駆動用モータ１₁ ，１₂ ，１₃ を制御するためのサーボドライバ２₁ ，２₂ ，２₃ が検出したモータの位置θｍ・速度θ_d ｍ・加速度θ_ddｍから、衝突が発生していない場合に各モータが発生すべきトルクＴ₁ ，Ｔ₂ ，Ｔ₃ を、ロボットの各関節の位置θ、速度θ_d 、加速度θ_ddとロボット機構パラメータとをもとにした
【数３】
で示す式に基づく逆動力学演算により予測する。そのトルク予測値と実際の関節駆動トルクＴｃとの差分ΔＴを求める。差分ΔＴの絶対値が予め設定された判定値εより大きい場合に衝突が発生したものと見做す。そして、複数の関節のうち停止もしくは低速で動作している関節あるいは慣性力が小さいため動作停止に必要なトルクが最大トルクと比較して十分小さい関節は、衝突があった後に衝突による干渉力を和らげるべく逃げ動作できるようにしたことである。
【００２９】
ロボットの停止処理をするにあたり、衝突発生時の各関節の速度と負荷イナーシャからロボットの動作を停止させるために必要な最小限のトルク値を関節ごとに算出し、その最小限のトルク値をサーボドライバのトルク制限器３に設定すると共に全てのサーボドライバの位置フィードバック比例ゲインＫppを０とし、強制的に速度指令値を０とした速度フィードバック制御による減速処理を行わせるようにする。
【００３０】
ロボットの動作を停止させるために必要な各関節のトルク値の算出にあたっては、全ての関節が同じ時間で停止すると仮定して、関節ごとに減速停止時間ｔmd₁ ，ｔmd₂ ，ｔmd₃ を算出したうえで、全ての減速停止時間のうちの最大値をいずれの関節の減速停止時間にも充てるようにすることが好ましい。
【００３１】
衝突が発生していない場合に各関節を駆動するモータが発生すべきトルクを予測するとき使用されるモータの位置・速度・加速度として、図９に示すように、サーボドライバ２₁ ，２₂ ，２₃ に位置指令すべく予め教示されている作業データの再生動作により演算した関節位置θと、その関節位置をもとにして計算された速度θ_d および加速度θ_ddを採用してもよい。
【００３２】
逆動力学演算においては、ロボットの関節数をｎとすれば、ロボットの各関節の加速度により各関節で発生するトルクならびに他の関節の加速度により発生する干渉トルクを示すｎ×ｎの慣性行列、遠心力とコリオリ力の影響を示すｎ×１の粘性行列、および重力加速度の影響により発生するトルクを示すｎ×３の重力行列が含まれる。
【００３３】
ｎ×ｎの慣性行列における対角要素としての「関節の持つ加速度が該関節自体に及ぼすトルクの関係を示した慣性モーメント」には、減速機を含むモータロータの慣性モーメントが減速比の２乗倍して加算され、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮できるようにしている。
【００３４】
逆動力学演算に供される式に、
【数４】
ただし、Ｃｍ_i 関節ｉにおける減速機の動粘性係数
（ｉ＝１，２，・・・ｎ 以下同じ）
θ_d ｍ_i 関節ｉにおけるモータの速度応答
Ｆcou _i 関節ｉにおける減速機のクーロン摩擦トルク
sgn （θ_d ｍ_i ） 符号関数
＝ １ ； θ_d ｍ＞０のとき
＝ ０ ； θ_d ｍ＝０のとき
＝−１ ； θ_d ｍ＜０のとき
を加算しておき、各関節内のモータ軸受や減速機の摩擦抵抗を考慮することができるようにしておくとよい。
【００３５】
ロボットの機構パラメータには、各関節に連結されるリンクの重心位置・質量・長さが含まれていればよい。さらに、慣性テンソルも付加されているならば、より一層好ましい。
【００３６】
トルク予測値との差分を求めるときの実際の関節駆動トルクには、実際にサーボドライバが生成したトルク指令値から算出されるトルクを充てておけばよい。
【００３７】
各モータにおけるトルク応答値から算出されるトルクを採用すれば、衝突検出精度はより一層高められる。
【００３８】
【発明の実施の形態】
以下に、本発明に係る多関節ロボットにおける衝突検出・停止制御法を、その実施の形態を表した図面を参照しながら詳細に説明する。まず、本発明が適用される例としてのアーク溶接ロボットの構成の概略、ならびにリンクを連結している関節を駆動するモータの制御から述べる。なお、本発明は、ロボットが作業中にワーク等の外界の対象物やその他の障害物に衝突したとき、いち早くかつ精度よく衝突のあったことを検出し、以後速やかにロボットの動きを止めたりリンクに作用する力の全部もしくは一部を解放して、ロボット本体やツールもしくはワークの変形や損傷等を軽減または回避できるようにしようとするものである。
【００３９】
詳細は図や表をもとにして後述するが、複数の関節からなるロボットにおけるいずれかのリンクの逃げ動作を概略的に示す図２を参照して、本発明の基盤となるところを説明する。この例は三つの関節７₁ ，７₂ ，７₃ からなるロボット１０であり、第１関節７₁ および第２関節７₂ の高速動作中に第３リンクＬ₃ が障害物８に衝突した場合を表している。述べるまでもないが、図中の矢印９のように、停止しているか動きの少ない第３関節７₃ が衝突による干渉力を和らげる方向に回転すれば、ロボット全体としての衝突による衝撃をおおいに緩和することができる。
【００４０】
ところで、従来技術の項でも触れたが、関節駆動トルクが非線形的に変動する複数の関節からなるロボットにおいては、衝突が発生していないときでも以下の要因を考慮して、各関節が発生するトルク値を正確に予測しておくことが重要である。それらをもとにして、衝突が原因で発生する関節駆動トルクの変動を検出すれば、衝突の発生したことを高精度かつ短時間に検出することができるようになるからである。
【００４１】
その要因とは、(1) ロボット各関節の位置の変化により生じる負荷イナーシャの変動（「イナーシャ変動」という）、(2) ロボット各関節の位置・加速度の変化により生じる他の関節の加速度運動から受ける干渉トルクの変動（「加速度の干渉」という）、(3) ロボット各関節の位置・速度の変化により生じる遠心力やコリオリ力からなる外乱トルクの変動（「速度の干渉」という）、(4) ロボット各関節の位置変化により生じる重力加速度の影響による外乱トルクの変動（「重力の干渉」という）の四つである。
【００４２】
本発明の最初の例を説明する制御ブロック線図は、図１に表されている。複数の関節を持つ図２に示したロボット１０のリンクＬ₁ ，Ｌ₂ ，Ｌ₃ やリンク先端に把持されたツールまたはワーク（図示せず）が、周囲の障害物８に衝突したときリンクやツールの変形を防止しさらには関節等のロボット可動部の損傷を回避すべく、図１中の衝突検出部１１で衝突したことを検出し、かつ直ちに停止処理部１２においてロボットを停止させることができるようになっている。なお、３つの関節それぞれのためにモータ１₁ ，１₂ ，１₃ が装備され、そのモータごとにロボットコントローラ１３からの指令を受けて動作するサーボドライバ２₁ ，２₂ ，２₃ が準備されている。
【００４３】
本発明に係る制御における衝突検出処理の主たるところは、以下の工程からなる。まず、ロボットの関節駆動用の各モータを制御するサーボドライバ２₁ ，２₂ ，２₃ が検出したモータの位置θｍ₁ ，θｍ₂ ，θｍ₃ 、速度θ_d ｍ₁ ，θ_d ｍ₂ ，θ_d ｍ₃ 、加速度θ_ddｍ₁ ，θ_ddｍ₂ ，θ_ddｍ₃ から、衝突が発生していない場合に各モータが発生しているはずのトルクＴ₁ ，Ｔ₂ ，Ｔ₃ を、後述する逆動力学演算によって予測する。
【００４４】
次に、そのトルク予測値Ｔ₁ ，Ｔ₂ ，Ｔ₃ と、実際の関節駆動トルク、すなわちサーボドライバ２₁ ，２₂ ，２₃ が実際に生成したトルク指令値Ｔｍ₁ ，Ｔｍ₂ ，Ｔｍ₃ から算出されるトルクとの差分ΔＴ₁ ，ΔＴ₂ ，ΔＴ₃ を求める。そして、いずれかの差分ΔＴ_i の絶対値が予め設定された該当する判定値ε_i より大きければ、衝突が発生したものと見做すようにしている。
【００４５】
衝突の検出は、ぶつかったことによるサーボの状態変数の変化があって始めて可能となることは、既に述べた。また、衝突を感知しているのは各モータ単位で検出されるトルクの変動であることも触れた。そのトルクの変動が起こるべくして起こったものか衝突によって起こったものかは、複数の関節があるとき上で述べた四つの外乱トルクの存在を見極めなければ判断することができない。しかしながら、その存在を見極めるためには、厳密な計算が求められることになる。
【００４６】
そこで、ロボットが所望の運動をするために必要な各関節に発生させるべきトルクを求め、これを実際にモータが発生しているトルクと突き合わせ、その違いから衝突の有無を検出することにする。演算量が多くなる傾向にはあるが、検出はより一層正確なものとなる。この例では、モータが励磁された後は何時でもサーボドライバからモータの現在位置・速度・加速度を取り込み、それらで各関節が動いているとして各モータに出ているはずのトルク値を計算するというものである。
【００４７】
ここで、まず図３を参照して、ロボットシステムの構成について説明する。ロボットは用途に応じて適宜の数の関節を備えるが、図の簡潔化を図るために既に示したとおり、３つの関節からなるロボットを例にする。ロボットの各関節を駆動するモータ１₁ ，１₂ ，１₃ のそれぞれには、回転による前後進量としての位置情報を提供するエンコーダ１ｅ₁ ，１ｅ₂ ，１ｅ₃ が取り付けられている。
【００４８】
これらのモータを駆動するための制御装置（ロボットコントローラ）１３はロボット本体とは別の制御ボックスに配備され、ティーチペンダント１４で指令された教示データを記憶し、その記憶データと予め設定記憶されている当該ロボット固有の機構パラメータ等とをもとにして動作を再生させる演算が行われるようになっている。
【００４９】
そして、コントローラ１３とモータ１との間には、各モータを位置・速度制御するサーボドライバ２₁ ，２₂ ，２₃ や、位置・速度制御により求められた指令電流値をもとにしてモータに電力を供給するサーボアンプ２ａ₁ ，２ａ₂ ，２ａ₃ も設けられる。ちなみに、サーボドライバ２の構成は従来の技術の項で述べた図１４のとおりであり、これが図１の下半部に記載しているかたちで搭載されている。
【００５０】
各サーボドライバには、制御装置から与えられる位置指令にモータを追従制御させるため、位置制御ループと速度制御ループとが組み込まれている。位置制御ループは比例要素によるフィードバック制御が、速度制御ループについては比例要素および積分要素によるフィードバックが行われる。なお、位置フィードバック比例ゲインＫppは、ロボットコントローラからの要求に応じて、任意の値に変更することができる。
【００５１】
速度制御系の後にはトルク制限器３が配置され、速度制御により生成されるトルク指令値Ｔｍ_i を任意の値で制限しておくことができるようになっている。トルク制限器３に設定される制限値も、コントローラ１３からの要求に応じて任意の値に変更可能である。なお、図中のＫvpは速度フィードバック比例ゲイン、Ｋviは速度フィードバック積分ゲイン、Ｓは微分要素、１／Ｓは積分要素を表している。
【００５２】
コントローラ１３は３つの関節に対する制御指令を発するもので、大別すると図３に表したように、中央処理装置ＣＰＵ，演算プログラムや各種データを記憶して演算する記憶演算部、教示データの取り込みや演算値の出力をするためのインターフェイスといったものからなる。ここで、教示されたデータ（以下作業データという）の再生動作を、以下に簡単に述べる。
【００５３】
制御プログラムはＲＯＭに格納されており、その中の軌道計画部２２が図１に示すようにハードディスク１５に格納されている作業データを読み出し、ロボット先端に把持されるツールまたはワークの作業軌道が三次元空間の直交座標上で計画される。
【００５４】
次いで、補間点計算部１６がその作業軌道を予め規定された時間の補間周期ごとに分割し、補間周期ごとにツール等が到達すべき位置・姿勢を補間点データとして計算する。そして、逆変換部１７が補間点データ中の直交座標で表された位置・姿勢を、多関節ロボットで再現するに必要な各関節レベルでの位置に逆変換し、各サーボドライバ２₁ ，２₂ ，２₃ への位置指令値を演算する。
【００５５】
このサーボドライバへの位置指令はＲＡＭに一時的に記憶され、ロボットの関節位置をサーボ位置指令値にデータ変換するデータ変換部１８から、同期信号Ｓ_YNC に同期して各サーボドライバへ伝達される。なお、以上の処理は、多関節ロボットをティーチングプレイバック方式で制御するうえで最も基本的な処理であり、公知であることは言うまでもない。
【００５６】
ちなみに、図３中のＲＯＭには、ロボットの動作制御ならびに衝突検出や停止処理を実行するプログラムと、その実行条件や制御定数などのデータが格納される。ＲＡＭはＣＰＵのワーキングエリアとして用いられ、計算途中のデータを一時的に保存する。ハードディスク１５には作業データのみならず、ロボット固有のデータ例えばリンクの長さ・質量といったものや、教示された動作において固有のものであるワークやツールに関するデータ等が格納される。
【００５７】
シリアルＩＯ（インプット・アウトプット）１９はティーチペンダント１４と接続され、ティーチペンダントの操作による作業データがシリアル通信によってＣＰＵで認識できるようになっている。サーボＩＦ（インターフェイス）２０はサーボドライバ２₁ ，２₂ ，２₃ に接続され、指令データの送信やモータの制御状態のモニタを可能にする。タイマ２１は、定期時刻ごとに同期信号Ｓ_YNC を発生し、この信号によってサーボドライバへの指令値の更新タイミングが与えられる。
【００５８】
次に、本発明によるロボットの衝突検出ならびに衝突後の停止処理を行うための最初の形態例を、図１に基づいて説明する。なお、停止処理は後述することにして、衝突検出から述べる。衝突の検出における基本的なところは、比較器２９₁ ，２９₂ ，２９₃ における比較の対象を、位置偏差ではなくトルクとしている点である。
【００５９】
ところで、トルクをもとにして衝突の有無を検出しようとする場合、大きいトルクが掛かっていても一概に衝突しているとは言えないことに留意しなければならない。関節が大きな加速度で動くときは大きなトルクが要求されるので、トルクが急激に大きくなったからといって衝突と見做すわけにはいかない。よって、本来所望する運動をさせるために必要なトルクは幾らか、今出ているトルクとはどの程度かけ離れているかを厳密に判定しなければ、高精度な衝突検出はできなくなる。本発明はこのような観点から研究を重ね、完成したものである。
【００６０】
まず、サーボドライバはモータの駆動を制御するだけでなく、サーボモータの状態をモニタできるものであるから、本例においてはサーボドライバからモータの位置・速度・加速度といったモータ情報を取り出すことにする。そして、これをコントローラ１３の衝突検出部１１に入力して、後で詳しく述べるＨ（θ）・θ_dd＋Ｃ（θ・θ_d ）＋Ｇ（θ）・ｇなる計算式をもとにした逆動力学演算を行い、衝突していなければ各関節駆動用のモータが発生するはずのトルクＴ₁ ，Ｔ₂ ，Ｔ₃ を予測する。
【００６１】
逆動力学演算とは、ロボットの各関節の位置・速度・加速度の状態と、ロボットの機構パラメータから、ロボットの各関節に発生するトルクを算出するものである。この逆動力学演算によれば、前記した(1) から(4) のトルク変動要因の影響を正確に予測することができるようになる。
【００６２】
一方、予測トルクと比較される対象の「実際に発生しているトルク」は、サーボドライバのトルクループの手前で拾い上げられたトルク値である。衝突がなければ、逆動力学で演算されたトルク値とほとんど同じであり、両トルクの差が大きければ衝突していることを意味することになる。従って、その差分が関節ごとに予め決められた後述する「しきい値」ε_i に達していれば、「衝突した」と見做すことにするのである。
【００６３】
Ｔ_i ＝Ｈ（θ）・θ_dd＋Ｃ（θ・θ_d ）＋Ｇ（θ）・ｇを計算するにあたり、そのロボット固有のデータすなわち機構パラメータも必要であり、それらがハードディスクから読み出される。トルク値は電流値でもＮ・ｍ（ニュートン・メータ）でもどちらでもよい。サーボドライバの中にトルク定数（Ｎ・ｍ／Ａ）を持ったトルク換算部があればＮ・ｍで出力することができ、それを演算値と突き合わせればよい。サーボドライバにトルク換算部がなければ、逆動力学演算において電流値に換算して比較すればよいことは言うまでもない。
【００６４】
各サーボドライバとロボットコントローラとの間にはサーボＩＦ２０が設置され、以下のデータが伝達される。図１に示されているように、サーボドライバ２からコントローラ１３へは、モータの位置応答θｍ［ｒａｄ］、モータの速度応答θ_d ｍ［ｒａｄ／ｓｅｃ］、モータの加速度応答θ_ddｍ［ｒａｄ／ｓｅｃ² ］およびモータへのトルク指令値Ｔｍ［Ｎ・ｍ］であり、コントローラからサーボドライバへは、位置指令値θ［ｒａｄ］、位置フィードバック比例ゲインＫppおよび後述するトルク制限値Ｔｍdec ［Ｎ・ｍ］である。
【００６５】
衝突の検出は、サーボドライバのフィードバック制御が行われている限り、図４に示す５つのステップが順次繰り返される。まず、ステップ１（図面では以下Ｓ１などと記す）では、衝突検出部１１がサーボＩＦ２０を介してサーボドライバ２から、ロボットの各関節を駆動するモータの位置・速度・加速度の応答値とトルク指令値を読み込む。この読み込みは、関節を駆動する全てのモータに対して行われる。３つの関節からなるロボットの場合では、モータの位置・速度・加速度の応答値ならびにトルク指令値は、表１の記号を用いて、それぞれ式(1-1) ないし(1-4) で表される行列となる。なお、式中の記号の添え数字は関節番号を表し、［ ］^T は表１の「個数」の欄に記載された行列を「転置」して表示したこと示している。
【表１】
【数５】
【００６６】
ちなみに、実施例では関節数を３としているが、本発明の趣旨からしてｎ≧２で適用することができる。ｎ＝１の場合も理屈上は不可能でないが、低自由度のロボットは通常速度は速くなくまた他関節からの干渉も受けないので、行列演算の必要性はない。なお、例えば一方にはツールが装着され他方にワークが把持された２基のロボットが連動して再生動作するシステムの場合であっても、本発明の思想は各ロボットごとに適用されるものである。
【００６７】
ところで、モータとロボット関節とは、減速機を介して結合されるのが一般的である。そのため、式(1-1) ないし(1-4) の値を、ロボット関節での位置・速度・加速度に換算する必要がある。関節における位置・速度・加速度の応答値ならびに駆動トルクは、表２の記号を用いてそれぞれ式(1-5) ないし（1-8)で表すことができる。
【表２】
【数６】
【００６８】
第ｉ関節を例にすれば、それらは式(1-10)ないし(1-13)で表される。位置・速度・加速度については減速比ζ_i で除した値が関節換算値となり、トルクにおいては減速比を乗じた値が関節換算値となる。そこで、ステップ２においては、モータレベルの位置・速度・加速度がロボットの関節レベルの値に変更される。
【数７】
【００６９】
関節レベルの位置・速度・加速度ならびにトルクの応答値が分かれば、ステップ３においては、これらを次に述べる逆動力学モデル（Ｈ，Ｃ，Ｇ）に適用した逆動力学演算によって、発生すると思われるトルクが計算される。逆動力学モデルには、慣性行列のＨ行列と粘性行列のＣ行列と重力行列のＧ行列とが含まれている。なお、各行列の物理的意味は後述する。
【００７０】
ロボットの機構パラメータ（関節に連結されるリンクの重心位置・質量・慣性テンソル・長さ等）と、各関節の位置・速度・加速度から、各関節に発生するトルクＴは式(2-1) により演算できることが知られている。これを逆動力学モデルという。
【数８】
【００７１】
式(2-1) は、広瀬茂男著「ロボット工学−機械システムのベクトル解析−」裳華房、第２版・1989年２月10日発行」（以下技術文献１という）に記載されている。その節11．３の〔４〕には、剛体の運動解析手法の一つであるニュートン・オイラー法を用いて逆動力学演算する手順が詳しく説明されているので、ここでは説明を省く。最終的に逆動力学演算式は技術文献１中の式(11.28) となるが、その式中の最後の項Ｊ^T Ｋ（転置したヤコビ行列にロボットの先端に掛かる外力と外力によるモーメントを表すＫを乗じたもの）は、本発明の場合、ロボットが衝突していない状態、すなわち空間を何物にも触れずに変位しているときのトルクを求めようとすることから考慮する必要がなく、従ってその項を省くことによって上記の式(2-1) を得ることができる。
【００７２】
関節数ｎのロボット場合、慣性行列Ｈはｎ×ｎの正則な対称行列であり、ロボットの各関節の加速度によって関節自体に発生するトルクならびに他関節の加速度によって発生する干渉トルクを意味する。粘性行列Ｃはｎ×１行列であり、ロボットの各関節の速度により生じる遠心力およびコリオリ力の影響で生じるトルクを示し、重力行列Ｇはｎ×３行列で、重力加速度の影響により発生するトルクを示すものである。
【００７３】
慣性行列Ｈについて、少し詳しく触れる。慣性行列Ｈ（後述する式(2-9) を参照）の対角要素のＨjjは、第ｊ関節の加速度が第ｊ関節自体に及ぼすトルクの関係を示す慣性モーメントを表しており、すなわちＨjjは関節位置により変動する負荷イナーシャとなる。その他の成分であるＨijは、第ｊ関節の加速度が第ｉ関節に及ぼす干渉トルクの関係を表す慣性モーメントである。
【００７４】
慣性行列Ｈ，粘性行列Ｃおよび重力行列Ｇは、ロボットの機構パラメータと各関節の位置・速度から計算できる。ロボットの機構パラメータは、以下の４つから構成される。なお、表３中の個数は、関節に連結されるリンク１つ当たりである。ちなみに、図２に表したロボットの場合の第３リンクＬ₃ に溶接トーチ等のツールが装着されているならば、その第３リンクのパラメータはツールが一体となった状態で与えられることになる。
【表３】
【００７５】
各リンクの質量・重心位置・慣性テンソル・長さ等のロボット機構パラメータは、ロボットの機構やリンク形状および材質から予め求めておくことが可能な既知の値であり、それがロボットの運動中に変化しないことも明らかであるから固定値であって、予めハードディスク１５の中に記憶される。それらの機構パラメータを読み出して式(2-1) に適用すれば、各関節の現在の位置・速度・加速度の３つと重力加速度ｇとから、各関節に発生するトルクを演算することができる。なお、式(2-1) の各要素は、表４に記載の意味を持つ。
【表４】
【００７６】
ここで、本例におけるロボットの機構パラメータについて説明を加える。質量ｍ_i 、重心位置ｍｘ_i ，ｍｙ_i ，ｍｚ_i 、慣性テンソルＩｘ_i ないしＩｚ_i は、第ｉの関節の動作により移動しかつ第ｉ＋１の関節の動作によっては移動しない第ｉの関節に連結されるリンクの質量、同じくリンクの重心位置、同じくリンクの慣性テンソルをそれぞれ示す。なお、慣性テンソルの演算手順は、技術文献１の節３．８に詳しく説明されているので（式(3.38)を参照）、ここでは説明を割愛する。
【００７７】
上記した重心位置および慣性テンソルを演算するうえでの基準は、リンク座標系に置かれる。関節ごとに割り当てるリンク座標系の定義手順が、吉川恒夫訳「ロボット・マニピュレータ」コロナ社、初版・1989年12月15日発行（以下技術文献２という）の節２．９に詳細に説明されているが、ここでは、基本的なリンク座標系の定義を図５において説明する。例えば、第２関節７₂ と第３関節７₃ に跨がるリンクＬ₂ においては、第２関節の回転中心を原点として第２関節に割り当られるリンク座標が定められ、そのｘ方向をリンクＬ₂ の長さ方向とし、ｚ方向を第２関節の回転方向、ｙ方向を右手座標系の方向と決める。
【００７８】
次に、慣性テンソルＩは一般に物体の回転しにくさを示すために使用され、よく知られているとおりの３×３の対称行列である式(2-2) で表される。その対角成分はロボット機構部の各関節の慣性モーメントを、その他の成分は慣性乗積の負の値を表すものとなっている。
【数９】
【００７９】
また、慣性モーメントは次式で求められ、物体（ロボットリンク）の回りにくさおよび止めにくさを表す。
【数１０】
【００８０】
次いで、慣性乗積は次式より求められ、物体（ロボットリンク）が回転運動を行っているとき、その運動を乱すモーメントを表す。
【数１１】
ここで、ｄｍは微小体積の質量を示す。また、式中のｘ，ｙ，ｚは、所定のリンク座標系を基準として表され、図５に示すリンクＬ₂ の慣性テンソルの基準座標は、第２関節に割り当てられたリンク座標系をリンクＬ₂ の重心位置に移したときの座標系とする。他の関節についても同様に定義する。
【００８１】
なお、慣性，粘性および重力の行列値の演算方法は技術文献１に詳しく説明されている。ここではその説明は省くが、３つの関節からなるロボットの場合、慣性，粘性および重力の行列は次式のように表される。
【数１２】
【００８２】
式(2-9) ないし式(2-11)の各構成要素は表３に示されたパラメータならびに関節位置θ，関節速度θ_d のみから構成され、θ，θ_d 以外はすべて既知の値である。従って、ステップ２で求められる関節位置応答θｒ，関節速度応答θ_d ｒを次式のごとくθ，θ_d に置けば、式(2-1) 中の慣性行列Ｈ，粘性行列Ｃおよび重力行列Ｇを求めることができる。
【数１３】
【００８３】
ステップ３でＨ，Ｃ，Ｇの各行列が算出されたことで、式(2-1) 中の未知の値は関節加速度θ_ddならびに重力加速度が働く方向を示す重力加速度ベクトルｇの二つとなる。後者はロボットの設置姿勢によって変わるものであるが、一般的にはロボットの設置姿勢が変わらないと考えて差し支えなく、従って予め規定しておくことができる。
【００８４】
ステップ４において、最終的に式(2-1) で表される逆動力学モデルを計算するうえで必要な値のうち未知なものは、関節加速度θ_ddだけとなる。これもステップ２で求められた関節加速度応答θ_ddｒを式(4-1) のかたちで代入すれば、衝突のない場合にロボットの各関節がθｒ，θ_d ｒ，θ_ddｒで運動するために必要となる各関節での発生トルクＴを、式(4-2) で予測することができる。
【数１４】
【００８５】
ステップ４で算出した衝突が起こっていない場合にロボットの各関節がθｒ，θ_d ｒ，θ_ddｒで運動するために必要な各関節のトルクＴと、ステップ２で算出した実際にモータが関節を駆動しようとしているトルク指令値Ｔｃとの差分ΔＴをステップ５において計算する。
【数１５】
【００８６】
衝突がなければ、トルク指令値Ｔｃと予測値Ｔとはほとんど等しく、ΔＴ≒０となるはずである。一方、ΔＴの絶対値が予め設定された判定値ε_i よりも大きくなる場合には衝突が発生したものと見做して、制御プログラムを停止処理へと移行させる。具体的には、式(5-3) ないし式(5-5) の条件式のいずれか一つでも満足する状態が生じれば、衝突が発生したと判定される。
【数１６】
【表５】
【００８７】
ちなみに、ε_i は例えば０．１Ｔｃ_i もあれば十分であることが多い。現実には０．３Ｔｃ_i なければならない場合もあるが、そのときでも３割増し以上のトルクが掛かれば衝突と判定することになるわけで、従来技術の項で述べた位置偏差による場合、トルクに換算すれば例えば０．８Ｔｃ_i もしくはそれ以上にも及ぶのに比べると、本発明の場合衝突検出精度が格段に向上することが分かる。このような向上は、関節によってはさらなる改善も期待できる。
【００８８】
なお、判定値ε_i は固定しておくといった単純な扱いをしてもよいが、加速度の大きさや速度の大きさに応じて可変とし、加速度や速度が大きければ感度を下げ、小さければ上げるといったように感度変更可能としておくこともできる。加減速時に感度を上げて０．１Ｔｃといったようにしておくと衝突の誤検出を招きやすく、加減速が少なくかつ低速のときに０．３Ｔｃといったように感度を下げておくと衝突の検出が遅れることになるからである。
【００８９】
以上の説明から分かるように、ロボットの衝突の検出のためには、全ての関節を駆動する各モータの情報を統括して処理し、それらを統合して逆動力学演算することにより全ての干渉（衝突）を見い出すことができるようにしている。その演算において、式(2-1) が不可欠であることは言うまでもない。ロボットの機構パラメータのうちリンク質量・リンク重心位置・リンク長さも欠くことのできない要素である。しかし、リンク慣性テンソルは細長いリンクを採用するロボットでは必要となるが、軽量ロボットまたは短リンクロボットなどの場合に考慮するに及ばないことがあるので、その都度適宜選択することにすればよい。
【００９０】
サーボドライバ２₁ ，２₂ ，２₃ に入力される「位置指令」はハードディスク１５に記憶されたデータに基づき軌道計画部２２で演算された結果として出された命令である（図１を参照）が、教示中にあってはティーチペンダント１４（図３を参照）によって入力されたデータに基づく「位置指令」ということになる。それゆえ、教示中の不意の衝突に対しても、本発明の動作を行わせることができる。
【００９１】
本発明に係る衝突検出法によれば、衝突検出が精度高く迅速になされるので、後述する停止処理操作とあいまって、少なくともロボットが壊れたり周辺に存在する治具等の機械的構成要素を損傷させたり、復旧に多大の時間を要することは可及的に少なくなり、衝突後の稼働や教示を速やかに再開することができるようになる。衝突検出はコントローラに搭載した衝突検出ソフトによるので、ショックセンサや機械式衝突検出装置をロボットリンクに装備する必要はなく、ロボットリンクの構造や機構の複雑化は回避され、また軽量化も図られる。
【００９２】
ところで、図１においては、サーボドライバからコントローラに、モータの位置応答，モータの速度応答，モータの加速度応答およびモータへのトルク指令値が伝達されると説明した。しかし、速度応答と加速度応答とはそれぞれの前者を微分すれば得られるものであるので、サーボドライバからコントローラへは、図６のようにモータの位置応答と速度応答にとどめ、衝突検出部１１において速度応答値を微分してモータの加速度応答を得るようにしてもよい。さらには、モータの位置応答のみをコントローラへ供給し、図示しないが、コントローラで一階および二階微分して速度および加速度を求めるようにすることも可能となる。いずれの場合も、サーボＩＦ２０を介した情報伝達の混雑を緩和させることが期待できる。
【００９３】
上記した例では、差分ΔＴを求めるにあたり、逆動力学演算に基づくトルク予測値と、サーボドライバが速度制御により生成したトルク指令値Ｔｍを実際の関節駆動トルクと扱い、そのトルク指令値を採用すると説明した。しかし、そのトルク指令値に代えて、各モータにおけるトルク応答値から算出されるトルクを採用してもよい。
【００９４】
図７から分かるように、サーボドライバ２₁ ，２₂ ，２₃ で求められたトルク指令値Ｔｍ_i に応じた電力を動力線から各ブラシ付きＤＣモータ１₁ ，１₂ ，１₃ に供給するサーボＡＭＰ２ａ₁ ，２ａ₂ ，２ａ₃ からの回路に電流検出器２３₁ ，２３₂ ，２３₃ を設ける。そして、電流検出器で拾った電流をＡ／Ｄ変換した後にそのトルク値Ｔｎ_i にトルク定数を乗じて、Ｎ・ｍとして得たトルク応答値（実際に出力されているトルク）Ｔｃ_i ’を使用する。この場合、前記した式(1-13)の代わりに次式が採用されることになる。
【数１７】
【００９５】
モータがブラシレスＤＣモータである場合には、実際に流れている全ての電流が有効なトルクになるわけではない。特にモータの高速回転時には、トルクに変換されない無効電流が増加し、指令と出力の誤差が大きくなる。実際にモータが出力しているトルクは、モータの三つ相のうち二つの相に流れている電流（以下相電流という）を検出して、図８に示す座標変換部２４で座標変換を行うことにより、トルクに変換される電流である有効電流ｉ_qaを求めることができる。
【００９６】
座標変換処理は無効電流を捨て有効分だけ取り出すというものであり、これについては、杉本英彦他著「ＡＣサーボシステムの理論と設計の実際」総合電子出版社、第２版・1990年５月８日発行（以下技術文献３という）の節４．２で詳細に説明されている。最終的には当文献中の式(4.11)で表される。式中のｉ_qaの項が有効電流に相当するもので、相電流ｉ_ua，ｉ_vaとモータの電気角θ_reより有効電流ｉ_qaが式(6-1) で算出される。
【数１８】
【００９７】
なお、電気角θ_reの検出には多くの方法があるが、絶対値エンコーダを使用する場合は次のようにして求めることができる。まず、電気角３６０°に相当するエンコーダのパルス変位θe _rev を算出する。θe _rev は、エンコーダ１回転当たりのパルス変位θｍ_rev をモータの極対数Ｐn で除した値であり、式(6-2) で算出される。極対数とはブラシレスＤＣモータのｕ，ｖ，ｗの三つの極対がモータ１回転当たりに配置されている数であって、一つの極対に相当するモータの回転量が電気角３６０°となる。エンコーダ１回転当たりのパルス変位θｍ_rev 、モータの極対数Ｐn についても、それぞれエンコーダとモータの仕様から予め規定しておくことができる値である。
【数１９】
【００９８】
エンコーダからの出力パルスθｅを電気角３６０°に相当するエンコーダのパルス変位θｅ_rev で除し、そのときの余りをθｅ_rev で除したものに２πを乗じることによって、電気角θ_reが求められる。具体的には、式(6-3) となる。式中の演算子ＭＯＤは、除算の余りを算出することを表している。
【数２０】
【００９９】
以上の式(6-1) ないし式(6-3) より算出された有効電流ｉ_qaにトルク定数Ｋt を掛けることにより、モータが実際に出力しているトルクＴｎが算出される。トルク定数Ｋt は有効トルクに対する発生トルクＴｎの比で、モータの仕様から予め規定しておくことができる値である。
【数２１】
【０１００】
上に現れた記号を参考までに、表６に掲記しておく。なお、上記した図８は出力トルクの検出機能を持つサーボドライバのブロック線図であり、電流検出器２３_u ，２３_v で検出された相電流ｉ_ua，ｉ_vaが、ＡＤ変換器を介して読みとられている。電気角の算出のため必要となるエンコーダの出力パルスも、読みとり可能となっている。
【表６】
【０１０１】
なお、モータがブラシ付きＤＣモータの場合には、図８中の座標変換は必要なく、直ちにトルク定数を乗じればよい。このようにモータの出力トルクを検出して、これを衝突検出に供するようにすれば、より一層検出精度が向上する。とりわけ、高速回転時にトルクに変換されない無効電流が増加するためにトルク指令値とモータ出力トルクとの間にずれの生じやすいブラシレスＤＣモータ等においては、極めて有効な手段となる。
【０１０２】
図１では、サーボドライバから拾い上げた位置・速度・加速度応答値を用いて逆動力学演算するようにしているが、拾い上げてから演算に入る関係上、後述する停止処理に入るまでに若干の遅れが出る。図９はそれを改善しようとするもので、衝突が発生していない場合に各関節を駆動するモータが発生すべきトルクを予測するときに使用されるモータの位置・速度・加速度として、作業データの再生動作のために演算した関節位置とその関節位置をもとにして計算した速度および加速度を採用するものである。
【０１０３】
モータがいかなる位置・速度・加速度で動いているかは軌道計画の段階で分かるはずであり、これを使用すればサーボの状態を見なくても予測トルクを計算しておくことができるとの知見に基づくものである。従って、軌道計画を解析した結果得られる位置・速度・加速度から各モータで発生すると思われるトルクを逆動力学演算によって求め、それを実際にモータに流れているトルクと突き合わせて衝突の有無を検出しようとするのである。
【０１０４】
そのために、図９のコントローラ１３内の制御プログラムには、図１に示した軌道計画部２２，補間点計算部１６，逆変換部１７に加えて、補間周期ごとのロボットの関節位置からロボット関節の速度・加速度を計算する速度・加速度計算部２５が付加されている。
【０１０５】
なお、データ変換部１８からサーボドライバ２への位置指令信号の出力と逆動力学演算のためのデータ信号２６の出力との間に時間的調整をとっておく必要があるが、その場合、後者を前者より例えばサーボドライバでの演算時間だけ遅らせるといった配慮をすればよい。この図９のような構成にすれば、サーボドライバ２とコントローラ１３との間のデータのやり取り量も少なくなり、通信の負担が軽減されることにもなる。ちなみに、ティーチペンダントによる教示中や手動運転中でも、軌道計画部２２で作業軌道を生成して関節の位置・速度・加速度を予測することが可能であるので、この方法は先の例（図１を参照）と同様に、動作再生時以外でも適用することができる。
【０１０６】
衝突の検出は、ロボットの作業軌道の補間点データに対してサーボドライバ２のフィードバック制御が行われている限り、図１０に示す６つのステップを順次繰り返して行うことにより実現される。なお、衝突検出は、コントローラ１３内のプログラムである衝突検出部１１で行われることは、図１の例と異なるところがない。
【０１０７】
まず、ステップ１１においては、ロボット各関節の位置・速度・加速度が、ハードディスク１５に記憶される作業データの再生動作行う過程で計算される。具体的には、速度・加速度計算部２５で補間周期ごとのロボットの各関節位置θからロボット各関節の速度θ_d ，加速度θ_ddが計算される。
【０１０８】
ここで、ロボット各関節の速度・加速度を計算する方法について説明する。補間周期Ｓmpt ごとのロボット各関節の位置θが算出されると、現周期で算出された関節位置をθcurrとし、直前の補間における関節位置をθprevとして、現補間点おける各関節の速度θ_d が以下のようにして算出される。
【数２２】
【０１０９】
現周期で算出された関節速度をθ_d currとし、直前の補間時の関節速度をθ_d prevとすれば、現補間点おける各関節の加速度θ_ddが同様の要領で求められる。なお、時刻０における、各関節の速度，加速度はいずれも０と置かれる。
【数２３】
【０１１０】
ステップ１１で算出したロボット各関節の位置θから、サーボドライバへの位置指令θc を式(8-1) により算出し、サーボＩＦ２０を介してサーボドライバ２へ出力する。モータは減速機を介して関節を駆動するので、ロボット各関節の位置θに減速比を乗じた値がサーボドライバへの指令値となる。このようにして、サーボドライバへの位置指令値の送信がステップ１２で行われる。なお、θc はロボット関節の位置応答 [ｒａｄ] であり、その個数はｎ（＝３）×１行列となる。
【数２４】
【０１１１】
ステップ１３ではトルクの状態の読み込みがなされる。すなわち、衝突検出部１１がサーボＩＦを介してサーボドライバ２から、ロボットの各関節を駆動するモータのトルク指令値Ｔｍ₁ ，Ｔｍ₂ ，Ｔｍ₃ を読み込む。また、モータからロボット関節への結合は減速機を介して行われるので、サーボドライバ内のトルク指令値Ｔｍ_i に減速比を乗じた値が関節を駆動するトルクＴc _i として与えられる。
【数２５】
【０１１２】
以後、ステップ１４では逆動力学モデル（Ｈ，Ｃ，Ｇ）が計算され、ステップ１５で逆動力学演算によるトルク予測値が演算される。ステップ１６で前述したとおりの手順によって衝突判定処理がなされる。もちろん、その後には後述する停止処理も行われる。
【０１１３】
図９の例においては位置・速度・加速度指令をもとにしてトルクを予測しており、実際の応答値と指令値の誤差が小さければトルク予測の精度が上がることは明らかである。従って、衝突検出精度も向上することになる。そこで、図９の構成に、サーボドライバ２内のサーボ制御系に対して速度指令をフィードフォワード補償すれば、サーボドライバの指令追従性能を上げることができ、常にロボットの衝突検出精度を高く維持しておくことができるようになる。図１１は、図９の構成に速度指令のフィードフォワード補償を付加した形態を示している。
【０１１４】
もう少し詳しく述べると、図９の場合、軌道計画の後に関節速度も演算されているので、図１１のように速度をデータ変換部２７からフィードフォワードすることができる。フィードフォワードすれば、位置・速度・加速度の関係が実際の応答と異なる程度を図９の場合よりも小さくすることができる。すなわち、図９の場合、位置・速度・加速度と実際に使用される位置・速度・加速度とが乖離する傾向にあり、従って衝突していないにもかかわらず逆動力学演算結果とトルク指令値とが一致しなくなるといった演算精度上の問題が生じる。しかし、フィードフォワード補償によれば、これを効果的に解決することができる。
【０１１５】
図１２には、速度指令のフィードフォワード補償の有無によって速度応答がどうのように相違するかが示されている。速度指令のフィードフォワード補償を行えば、（ａ）と（ｂ）を見比べて分かるように、破線で示す速度指令に対して実線で示すごとく追従性能（速度応答）が向上する。これから分かるように、フィードフォワードすればタイムラグが小さくなり、結果として、位置・速度・加速度の関係も実際に極めて近いものとなる。
【０１１６】
なお、フィードフォワード補償すると位置フィードバック比例ゲインＫppの効きは落ちるが、速度はフィードフォワード的に効くので速度指令が迅速になされる。ただ速度だけでは十分な位置精度が出ないので位置をもとにした速度制御も残されており、これを速度のフィードフォワードに対して補間的に機能させておくことになる。
【０１１７】
ちなみに、式(2-1) はＪ^T Ｋを省いたものであると説明したのは、常に外部の何物にも触れずに作業しまた空間を変位する例えばアーク溶接ロボットに適用する場合を念頭に置いたからである。そこで、本発明をワークに対して接触しながら作業する類のロボットに適用する場合は、ツールを現在位置から次の目標位置まで単に空間移動させるといったときのみ有効となる。しかし、式(2-1) にＪ^T Ｋを付加しておくならば、逆動力学演算に多少の処理時間増加を伴うことになるが、力覚センサ等を働かせながらワークに対して接触する作業をしているときも本発明の思想を反映させることができるようになる。
【０１１８】
以上までは衝突の検出方法を述べたが、これからは衝突の検出があった後の停止処理について説明する。その処理の趣旨は、以下のとおりである。衝突が発生したときは、いち早くそれを検出すべきであることは言うまでもない。しかし、衝突を検出できた時点では衝突してから既に少し時間が経過しているわけであるから、衝突直前にロボットを停止させるということは至難の技である。
【０１１９】
そこで、ロボットの停止操作においては、衝突してもロボット本体やぶつかった対象物や障害物の損傷が最小限に抑えられるようにすることを最終的な目標とすべきである。簡単に言えば、各モータには、その関節を介して動かされているリンクが今持っている慣性モーメントを吸収して停止させることができる程度のトルクしか発生できないようにしておき、これによって衝突逃げ動作を可能にして被害を最小限にくい止めるとの観点に立った停止処理を行うことにする。
【０１２０】
この停止処理は図２のところでも触れたように、停止もしくは低速で動いている関節については、矢印９のような衝突による干渉力を和らげる方向への回転を許容できる制御をすることであり、従来技術の項で述べたように、どの関節においても可能なかぎり大きいトルクを発生させて迅速に制動しようとする場合、ロボットと障害物との間に大きな力が作用することになるというのを回避しようとするものである。
【０１２１】
具体的には、衝突検出できた以降は、各モータ速度を０とするために必要な最小限のトルクを越えるトルクを発生させないように、モータの発生トルクを制限することにする。例えば第３関節は止まっておりしかも重力も作用していないとするとトルクはほとんど０でよいから、ぶつかればその関節は簡単に回転し、リンクは障害物からスルッと逃げる。負荷を伴うことなく逃げることができれば、第２関節や第３関節には停止するための惰走の余地も残され、ロボット全体の干渉力や衝撃力を和らげ被害の発生が最小限に抑えられる。このように、本発明においては、大きな慣性力を持っていないかまたは高い速度が発生していない関節については、力に逆らわずに逃げる仕組みを提案しようとするものである。
【０１２２】
その停止処理の主旨は、関節のうち停止もしくは低速で動作している関節あるいは慣性力が小さいため動作停止に必要なトルクが最大トルクと比較して十分小さい関節については、衝突があった後に障害物に対して逃げ動作をさせ、衝突による干渉力が和らげられるようにしたことである。
【０１２３】
また、衝突発生時の各関節の速度と負荷イナーシャからロボットの動作を停止させるために必要な最小限のトルク値を関節ごとに算出し、その最小限のトルク値をサーボドライバ内のトルク制限器３に設定すると共に、全てのサーボドライバ内の位置フィードバック比例ゲインＫppを０に置き替えるようにする。それによって、強制的に速度指令値を０とした速度フィードバック制御による減速処理を行わせるのである。
【０１２４】
さらに、ロボットの動作を停止させるために必要な各関節のトルク値の算出においては、全ての関節が同じ時間で停止すると仮定し、関節ごとに減速停止時間を算出した後、全ての減速停止時間のうちの最大値をいずれの関節の減速停止時間にも充てることにより、上記した逃げ動作を幾つかの関節において実現できるようにするものである。
【０１２５】
衝突が検出されても速度はステップ状に０となることがないので、衝突発生時の関節速度θ_d ｒはサーボドライバの状態変数であるモータ速度θ_d ｍから知ることができる。関節位置についても、サーボドライバの状態変数であるモータ位置θｍから知ることができる。衝突発生時の関節位置が分かれば、負荷イナーシャであるＨ行列の計算は可能となる。これは当然ロボットの姿勢によって変化するイナーシャである。この衝突発生時のイナーシャと速度から以後速度を０にするに必要なトルクを計算し、この値以上のトルクが発生しないようにモータ出力トルクを制限する。このトルク出力制限処理は、図１，図９，図１１に表されたトルク制限値の伝達経路２８を介して行われる。
【０１２６】
図２のように第３関節７₃ は動いていないが第１関節７₁ と第２関節７₂ が動いているとすると、第１関節と第２関節についてはその慣性力を制御するだけの比較的大きなトルクが必要となるが、第３関節では慣性力は小さく従って減速に必要なトルクも第１関節や第２関節に比較すれば小さくてよい。そのため、第３関節７₃ を駆動するモータのトルク制限器には小さい値が設定され、第３関節に連結されたリンクが第１関節，第２関節の動きによって衝突しても、第３関節は簡単に衝突による干渉力を和らげる方向へ逃げることが可能となり、衝突による変化を吸収することができる。
【０１２７】
以下に、停止処理の具体例を述べる。前述したいずれかの衝突検出処理により衝突を検出した後、図１３の４つのステップからなる処理を行う。まず、ステップ２１において、ロボットの動作停止に使用可能なトルク値を算出する。
【０１２８】
衝突発生時の停止処理に使用可能な各関節のトルク値Ｔusabが、式(4-2) を変形した式(10-1)から算出することができる。このトルク値Ｔusabは、動作を停止させるための減速運動に使用できるもので、加速度によって生じるトルクと見做される。重力によるトルクや粘性によるトルクは速度に依存するものであり、加速度によるトルクとは別に確保されねばならないトルクである。
【０１２９】
式(4-2) を変形して速度により消失するトルク（粘性トルク）と重力により消失するトルクとを引き去れば、それが減速に供し得るトルクとなる。すなわち、遠心力や重力加速度に影響する外乱トルクによって、実際に停止のための減速運動に使用できるトルクが制限される。
【数２６】
【０１３０】
なお、式(10-1)中のsign（θ_d ｒ）は符号を決めている。Ｔmax は各モータが発生し得る最大トルクであって、正の値に設定される。しかし、場合によってはマイナス値として使用したいときもあるので、sign（θ_d ｒ）はそれを判定している。現在の速度が正であれば負の加速度が必要となり、負のトルクが与えられる。正の速度が出ているとＴmax は正となるが、負の符号がついているので負のトルクとなる。負の速度が出ていれば正の加速度が必要となるので、負×負＝正となるようにしている。すなわち、速度（θ_d ｒ）に対して符号を反転させることを意味している。
【０１３１】
−sign（θ_d ｒ）・Ｔmax は、各関節が出力できるトルクの最大の減速トルクである。現在の速度を０にするために必要な減速トルクは、そのトルクから速度によって消失するトルクと重力によって消失するトルクを引算したもので、それが実際に減速のために使うことができる式(10-1)で与えられるトルクＴusabである。
【０１３２】
式(10-1)は、式(4-2) のＨ（θｒ）・θ_ddｒをＴusabと、式(4-2) のＴを−sign（θ_d ｒ）・Ｔmax と置き換えたものである。なお、第ｉ関節の最大出力トルク値Ｔmax _i は、関節を駆動するモータが出力可能な最大トルクをＴｍ maxとすると、減速比を考慮して式(10-4)で算出することができる。言うまでもなく、Ｔｍ maxはモータの仕様から予め規定できる値であり、粘性行列Ｃならびに重力行列Ｇは、衝突発生時の各関節の位置と速度から算出可能である。
【数２７】
【０１３３】
Ｔｍ max_i も減速比ζ_i も既知の値であるからＴmax _i も予め固定的に与えるておくことができる。Ｃ行列やＧ行列も衝突を検出する過程で計算されており、新たに計算する必要はない。現時点のものでよいから、式(4-2) で使ったものをそのまま持ってくればよい。本当は時間のずれが極く僅か存在するが、衝突した時から再度計算するには及ばないからである。もちろん、計算してもよいが時間を要するうえに計算値に大した違いは生じない。なお、上に現れた記号を参考までに、表７に掲記しておく。
【表７】
【０１３４】
ステップ２２では、動作を停止させるために必要な各関節のトルク値が算出される。減速により発生するトルクは、慣性行列Ｈと各関節の減速度の積であり、式(11-1)で表される。なお、θ_ddは未知であるが、分かっていればこの式によってＴdec が求まることになる。
【数２８】
【０１３５】
ところで、この停止処理の制御においては、全ての関節が時間ｔmdで速度０に減速停止するものと仮定する。すると、各関節の減速度θ_ddは、式(11-2)で表現することができる。なお、式(11-2)中のθ_d ｒは、衝突発生時の各関節速度である。ただ、いまの段階ではｔmdは未知である。
【数２９】
【０１３６】
全ての関節が式(11-2)の時間ｔmdで減速停止することを制御条件としているのは、何らかの条件を課さなければ制御の実行が図られないためであるが、この条件によって、幾つかの関節において上記した逃げ動作を可能ならしめることができるようになるのである。
【０１３７】
そこで、時間ｔmdとして、後で説明する式(11-4)ないし式(11-6)でロボットの関節の数だけ求まる複数のｔmdの中から、最長のものを使用することにする（式(11-7)を参照）。これは、ロボットの全ての関節において衝突後のロボットの減速運動により発生するトルクが、減速に使用可能なトルクＴusabを越えないようにしておくためであり、それを実現するｔmdによって、全ての関節を同じ時間で停止させることができるようになる。
【０１３８】
なお、参考までに記せば、選定されたｔmdの関節は、結果的に最も速く動いている関節あるいは一番大きいモーメントを受ける関節であることが多い。ちなみに、このようにして選定された時間によれば、最も遅く動いている関節では速度が０になるまでに時間が十分にあり無用な惰走が強られることになるが、元来速度は低いので多少惰走してもその距離がさして大きくなることはない。
【０１３９】
ここで、停止処理の減速に使用できるトルクは、式(10-1)からＴusabと算出されているので、式(11-1)の右辺をＴusabに置き換え、全ての関節が式(11-2)の時間ｔmdで減速停止するものとする。式(11-1)中のθ_ddに式(11-2)の右辺を代入すると、式(11-1)は式(11-3)のように変形される。式(11-3)中の慣性行列Ｈの算出方法は、既に説明したとおりである。
【数３０】
【０１４０】
この式(11-3)から、停止処理の減速に使用できるトルクＴusabの範囲内で、ロボットの各関節の速度を０にできる減速停止時間ｔmdが算出できる。なぜなら、式(11-3)において、Ｈ（θｒ）は、衝突発生時の各関節位置θｒから算出できかつθｒはサーボＩＦ２０を介してモニタ可能であるからである。また、θ_d ｒは衝突発生時の各関節速度でありサーボＩＦを介してモニタ可能であり、Ｔusabは式(10-1)で計算済みでもある。
【０１４１】
最終的には未知の値はｔmdだけとなる。ｔmdは行列式であり、３つの関節からなるロボットの場合には、３つの関係式ができる。実際には、式(11-3)を、関節ごとに以下の３つの式に分解し、減速停止時間ｔmdを算出する。具体的に言えば本実施例では関節数ｎ＝３であるから、関節ごとに減速停止時間ｔmd₁ ないしｔmd₃ を算出した後に、ｔmd₁ ，ｔmd₂ ，ｔmd₃ の中の最大値を全ての関節の減速停止時間に充てる。式(11-7)中のＭＡＸは要素中の最大値を抽出することを意味している。
【数３１】
【０１４２】
動作を停止させるために必要な各関節のトルク値Ｔdec _i は、式(11-4)ないし式(11-7)から算出される減速停止時間ｔmdを用いて、式(11-8)で算出される。なお、Ｔdec _i は停止に必要な関節のトルク値 [Ｎ・ｍ] で、その個数はｎ（＝３）×１行列である。ｔmdは全ての関節での減速停止時間 [ｓｅｃ] で、個数は１である。
【数３２】
【０１４３】
このようにして算出されたＴdec は、衝突発生時のロボットの各関節の位置・速度により変化する負荷イナーシャや外乱トルクの変動を考慮したもので、停止時間に遅延を生じさせない必要最小限のトルク値となる。
【０１４４】
式(11-1)のＨ行列，式(11-2)の現在の関節速度θ_d ｒおよび減速停止時間ｔmdから、各関節に必要な減速トルクＴdec が式(11-8)により分かる。どこか一つの関節すなわち減速動作に最大時間を要すると思われる関節についてはＴusabに等しくなるが、他の関節ではＴdec ＜Ｔusabとなる。これは速度が小さいか慣性力（＝Ｈ）が小さいものについてはＴdec も小さくなるからで、これがトルク制限値として設定されると、干渉力に対して逃げ動作を可能にする。
【０１４５】
ステップ２３では、サーボドライバ２内のトルク制限器３の設定値が変更される。すなわち、ステップ２で算出したＴdec は関節で発生するトルクであり、実際にモータで発生するトルクの制限処理を行うのはサーボドライバで行われる。通常、モータは減速機を介して関節を駆動しているので、Ｔdec をモータシャフトの換算値Ｔｍ decに変更し、サーボＩＦ２０を介した伝達経路２８を通じてＴｍ decが各サーボドライバのトルク制限器３に設定される。
【０１４６】
第ｉ関節を駆動するモータにおける停止時間に遅延を生じさせない必要最小限のトルク値Ｔｍ decｉは式(12-1)のようにして算出することができる。なお、Ｔｍ decは停止に必要なモータのトルク値 [Ｎ・ｍ] であり、その個数はｎ（＝３）×１行列となる。
【数３３】
【０１４７】
ちなみに、トルク制限値として式(11-8)のＴdec がサーボドライバに入ってくるが、Ｔdec はそのまま入れられない。動力学演算というのは一般的に関節レベルで得られるので、求まったＴdec も関節レベルのトルクであって、実際にトルクを制限するのはモータである。モータと関節との間には減速機が介在されているから減速比で割った値を設定しなければならない。従って実際はＴｍ dec_i が入れられることになる。
【０１４８】
念のために記載するが、Ｈ行列やθ_d ｒが関節ごとに違うから、ｔmd₁ ＝ｔmd₂ ＝ｔmd₃ としているとはいえ、Ｔｍ dec_i はサーボドライバ２₁ ，２₂ ，２₃ ごとに異なる。なお、得られたＴｍdec _i をそのまま使用してもよいが、Ｔｍdec _i に対して関節ごとにウエイトづけすれば都合のよい場合がある。そのような場合には、異なった係数を各Ｔｍdec _i に乗じて採用すればよい。
【０１４９】
ステップ２４では、各サーボドライバ２内の位置フィードバック比例ゲインＫppが、サーボＩＦ２０を介して０に変更される。以上のステップ２１ないし２４の処理により、強制的に速度指令値を０とした速度フィードバック制御による減速処理が行われる。
【０１５０】
各サーボドライバ内のトルク制限器３には、停止に必要な最小限のトルク値が設定されるので、衝突による干渉力がこれを上回る場合、関節が干渉する力（衝突力）を逃がす方向に動作し衝撃が軽減される。トルク制限器に最大トルク値が設定される従来の場合に比べれば、干渉力緩和効果が飛躍的に改善されることになる。
【０１５１】
このように、トルク制限値を置き替えるだけでなくＫppを０にしているのは、トルク制限値の置き替えだけでは関節が止まらないからである。位置指令や位置のフィードバックに如何なる値が来ていようとも、Ｋppを０にすれば速度指令は直ちに０となる。速度指令を単純に０とするには、Ｋppを０にすることが手っとり早いからである。
【０１５２】
上記から分かるように、速度ループだけで関節の動きを止めにいこうとするのであるが、トルクが適当に変更して制限されているので、速く動いている関節では最大トルクで止める制御がなされる。一方、他の関節では、自己の速度を０に保持するだけのすなわち現在の姿勢を維持するだけのトルクしか発生しないようにしておくことができる。
【０１５３】
これから分かるように、上記の停止処理によれば、トルク制限値を越える干渉力や衝撃が作用した場合に、その力の優劣を利用して関節に力逃しの挙動をさせることができ、ぶつかったことによる損傷の発生が可及的に抑えられる。なお、停止した状態でも、障害物にぶつかった関節であれぶつかっていない関節であれ速度ループにおける積分項はいつまでも機能するので、各リンクは関節が発揮している制限値内のトルクによって姿勢を維持し続ける。
【０１５４】
ちなみに、例えば水平面内のみで変位するリンクであって、姿勢保持に重力に対抗する力を必要としないロボットの場合やトルクが掛かり放しでは不味い境遇にあるリンクなどでは、速度が０になった時点でトルクを解放すればよい。その場合にはＫviの入っている積分項の働きをなくせばよく、Ｋppを０にするときもしくはその後に積分項をクリアする操作を付加しておけばよい。
【０１５５】
以上で、衝突検出後の停止処理の説明を終えて、以下に、衝突検出精度の向上策について述べる。既に述べたように、トルク指令値に代えてトルク応答値を採用することもその一つであるが、ここではモータロータの慣性モーメントを考慮したり、モータ軸受や減速機の摩擦抵抗を考慮することによる衝突検出精度の向上について触れる。いずれも単独もしくは重複して機能させることができるものである。
【０１５６】
まず、モータロータの慣性モーメントの考慮による衝突検出精度の向上から説明する。これは、前記したｎ×ｎの慣性行列における対角要素としての「関節の持つ加速度が該関節自体に及ぼすトルクの関係を示した慣性モーメント」、すなわち「関節位置により変動する負荷イナーシャ」に、減速機を含むモータロータの慣性モーメントを減速比の２乗倍して加算しようとするものである。これによって、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクも考慮して、衝突検出精度を向上させることができる。
【０１５７】
まず、式(2-9) の慣性行列Ｈを以下のように変形することにより、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮に入れる。ここで、Ｉｍはモータロータの慣性モーメント [ｋｇ・ｍ²]で、その個数はｎ（＝３）×１行列となる。
【数３４】
【０１５８】
図１や図９さらには図１１の例に共通して言えることであるが、慣性行列Ｈは各関節に連結されたリンクのイナーシャだけを計算しているに過ぎない。関節はモータで駆動されるから動くのであるが、関節を駆動する以前にモータも駆動されており、そのときのロータ自体の回転の慣性にも注目すべきである。
【０１５９】
ここで、Ｉｍ_i は減速機を含むモータロータの慣性モーメントである。連動して動く回転体の慣性モーメントは、その減速比の２乗分の１倍されて入力軸の慣性モーメントに影響を与える。従って、入力軸の慣性モーメントが出力軸に与える影響はその逆であるため、減速比の２乗倍されなければならない。減速機を含むモータロータの慣性モーメントが減速比の２乗倍されて加算されているのはそのためである。
【０１６０】
慣性行列Ｈの対角要素はそれ自体に与えるトルクの影響を示し、他の要素は他の関節に与える干渉トルクの影響を示す。減速機を含むモータロータの慣性モーメントが他の関節と干渉することはないので、対角要素のみを修正すればよい。なお、Ｉｍ_i もξ_i も予め設定可能な既知であるから、式(13-1)による場合も、既に述べた例と同様に衝突検出ならびに停止処理は可能である。
【０１６１】
次に、摩擦抵抗を考慮して衝突検出の精度を上げることについて説明する。式(2-1) で表されるロボットダイナミクスモデルを、式(14-1)のように変更することにより、各関節内のモータの軸受や減速機の摩擦抵抗を考慮できるようにしようとするものである。なお、ここで現れる記号を参考までに、表８に掲記しておく。
【数３５】
【表８】
【０１６２】
減速機自体にも摩擦がかなりあり、これも無視し得ない。速度項のことを粘性行列というが、それがＣｍ・θ_d である。しかし、これは物が擦れるときの摩擦を対象とするものではない。或る速度で動いているものが或る速度で動いているものに重なると、その速度と速度が干渉しあって別方向にトルクが生じるが、そのトルクを意味する。これは、式(14-1)の４項目に表されている。
【０１６３】
Ｃｍは動粘性係数であり、これに速度を乗じれば、外乱トルクとなる。｜Ｆcou ｜はクーロン摩擦（静摩擦：動き始めるまでの抵抗）と言われるもので、速度に依存することなく一定の大きさを持つが、常に運動と逆の方向に働くトルク成分である。Ｃｍ・θ_d ｍと｜Ｆcou ｜との和が、ここで考慮すべき摩擦トルクである。式(14-1)は式(2-1) に４項目を追加したものであり、摩擦を考慮した演算で精度を上げようとするものである。
【０１６４】
ここで、Ｃｍはモータの軸受けや減速機の動粘性係数ｎ×１行列であり、動粘性係数とは、速度と動摩擦トルクとの比例関係を表す定数である。また、動摩擦トルクとは、速度に比例した大きさを持つトルク成分である。さらに、θ_d ｍはモータの速度ｎ×１行列である。なお、式(14-1)中のsgn ( θ_d ｍ）は、モータの速度に基づいて、次式のように決定される符号関数である。
【数３６】
【０１６５】
また、関節速度とモータ速度の関係はその減速比ξから、式(14-5)のように表される。
【数３７】
ここで、ξは関節内の減速機の減速比である。パラメータＣｍ，Ｆcou ，ξも予め設定可能な既知の値であるから、上記(14-1)によっても衝突検出ならびに停止処理できることは述べるまでもない。
【０１６６】
【発明の効果】
本発明によれば、サーボドライバが検出したモータの位置・速度・加速度から衝突が発生していない場合に各モータが発生すべきトルクを予測し、そのトルク予測値と実際の関節駆動トルクとの差分を求め、その絶対値が予め設定された判定値より大きい場合に衝突が発生したものと見做すようにしたので、ロボットのいずれかのリンクやツールがワーク等の対象物やその他の障害物に衝突したことをトルクを通じていち早くかつ正確に検出することができるようになる。
【０１６７】
衝突が生じていない場合に各関節を駆動するモータが発生すべきトルクを、ロボットの各関節の位置θ、速度θ_d 、加速度θ_ddとロボットの機構パラメータとをもとにして、Ｔ＝Ｈ（θ）・θ_dd＋Ｃ（θ，θ_d ）＋Ｇ（θ）・ｇの逆動力学演算によって予測するようにしているので、そのトルク予測値を高精度に求めておくことができ、衝突センサ等の機械的構成物を搭載しなくても、迅速かつ正確に衝突を検知できる。
【０１６８】
関節のうち停止もしくは低速で動作している関節あるいは慣性力が小さいため動作停止に必要なトルクが最大トルクと比較して十分小さい関節には、衝突があった後に逃げ動作させるようにしておくので、衝突による干渉力を軽減して物損の発生を抑制することができる。
【０１６９】
衝突発生時の各関節の速度と負荷イナーシャからロボットの動作を停止させるために必要な最小限のトルク値を関節ごとに算出し、その最小限のトルク値をサーボドライバ内のトルク制限器に設定すると共に全てのサーボドライバ内の位置フィードバック比例ゲインＫppを０とし、強制的に速度指令値を０とした速度フィードバック制御による減速処理を行わせるようにすれば、位置指令や位置のフィードバックに如何なる値が来ようとも、速度指令を単純に０にしておくことができ、加えて、衝突による干渉力がトルク制限値を上回る場合、関節が干渉力を逃がす方向に動作して衝突力が軽減される。
【０１７０】
ロボットの動作を停止させるために必要となる各関節のトルク値の算出においては、全ての関節が同じ時間で停止すると仮定し、関節ごとに減速停止時間を算出した後、全ての減速停止時間のうちの最大値をいずれの関節の減速停止時間にも充てるようにすれば、衝突しても逃げ動作する余地が残され、衝突による力を吸収して損傷の発生を可及的に少なく抑えることができる。
【０１７１】
トルクを予測する際に使用するモータの位置・速度・加速度として、サーボドライバに位置指令すべく作業データの再生動作において演算した関節位置と、その関節位置をもとにして計算された速度および加速度をあてがうようにすれば、サーボドライバからモータの位置・速度・加速度の情報を取り込む時間等だけ迅速に衝突検出演算を行わせることができる。
【０１７２】
逆動力学演算においては、ロボットの各関節の加速度により各関節で発生するトルクならびに他の関節の加速度により発生する干渉トルクを示すｎ×ｎの慣性行列、遠心力とコリオリ力の影響を示すｎ×１の粘性行列、および重力加速度の影響により発生するトルクを示すｎ×３の重力行列を含むことにしているので、その演算精度は極めて高いものとなる。
【０１７３】
ｎ×ｎの慣性行列における対角要素としての「関節の持つ加速度が該関節自体に及ぼすトルクの関係を示した慣性モーメント」に、減速機を含むモータロータの慣性モーメントを減速比の２乗倍して加算しておけば、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮した衝突検出を行うことができる。
【０１７４】
逆動力学演算に供される式に、減速機の動粘性係数とクーロン摩擦トルクの要素を反映させるようにしておくと、各関節内のモータ軸受や減速機の摩擦抵抗を考慮した精度の高い衝突検出が実現される。
【０１７５】
逆動力学演算に供されるロボットの機構パラメータとして、関節に連結されるリンクの重心位置・質量・長さを与えておけば、演算が個々のロボットに則したものとなり、ましてや、慣性テンソルも付加しておけば、より一層精度のよいトルク予測が可能となる。
【０１７６】
トルク予測値との差分が求められる実際の関節駆動トルクとして実際にサーボドライバが生成したトルク指令値から算出されるトルクを使用すれば、演算速度を高く維持することができる。各モータにおけるトルク応答値から算出されるトルクとする場合には、現実のトルクとの比較になって衝突検出のための精度がさらに向上する。
【図面の簡単な説明】
【図１】 本発明に係る多関節ロボットにおける衝突検出・停止制御法が適用されているロボット制御系全体を示したブロック線図。
【図２】 関節数を３として簡略化した構造のロボットを表すと共に、リンクが障害物に当たったときの逃げ動作を表したロボットの概略図。
【図３】 ロボットコントローラ（制御装置）とティーチペンダントならびに関節駆動用モータとの間の信号伝達の概要を示した制御ブロック図。
【図４】 衝突の有無を判定する処理手順を表したフローチャート。
【図５】 イナーシャテンソルの基準座標を与えた第２リンクの説明図。
【図６】 サーボドライバからコントローラへ伝達される応答値の変更例を含んだ制御系全体を示すブロック線図。
【図７】 トルク指令値に代えてトルク応答値を採用する場合のサーボドライバのブロック線図。
【図８】 ブラシレスＤＣモータを使用している場合のサーボドライバのブロック線図。
【図９】 軌道計画によって生成された関節位置から得られる速度・加速度をもとにして逆動力学演算させるようにしたロボット制御系全体を示すブロック線図。
【図１０】 衝突の有無を判定するにおいて図９に基づく処理手順を採用した場合のフローチャート。
【図１１】 速度をサーボドライバにフィードフォワードした場合のロボット制御系全体のブロック線図。
【図１２】 速度指令に対する速度応答の遅れの程度を表したものであり、（ａ）は速度をフィードフォワードしない場合の応答図、（ｂ）はフィードフォワードした場合の応答図。
【図１３】 停止処理する手順を表したフローチャート。
【図１４】 モータを駆動するための各種制御ループが組み込まれたサーボドライバの構成概略図。
【図１５】 サーボドライバから位置偏差を取り出して衝突の有無を判定すると共に、ひき続いて停止処理することもできるようになっていることを表した従来技術のブロック線図。
【符号の説明】
１，１₁ ，１₂ ，１₃ …モータ、２，２₁ ，２₂ ，２₃ …サーボドライバ、３…トルク制限器、７₁ …第１関節、７₂ …第２関節、７₃ …第３関節、８…障害物、１０…ロボット、１１…衝突検出部、１２…停止処理部、１３…コントローラ、１５…ハードディスク、２２…軌道計画部、２３…電流検出器、２５…速度・加速度計算部、２９₁ ，２９₂ ，２９₃ …比較器、Ｌ₁ …第１リンク、Ｌ₂ …第２リンク、Ｌ₃ …第３リンク、θｍ₁ ，θｍ₂ ，θｍ₃ …サーボドライバが検出したモータの位置、θ_d ｍ₁ ，θ_d ｍ₂ ，θ_d ｍ₃ …サーボドライバが検出したモータの速度、θ_ddｍ₁ ，θ_ddｍ₂ ，θ_ddｍ₃ …サーボドライバが検出したモータの加速度、Ｔ₁ ，Ｔ₂ ，Ｔ₃ …衝突が発生していない場合に各モータが発生しているトルク予測値、Ｔｍ₁ ，Ｔｍ₂ ，Ｔｍ₃ …サーボドライバが実際に生成したトルク指令値、ΔＴ₁ ，ΔＴ₂ ，ΔＴ₃ …トルク予測値とトルク指令値から算出されるトルクとの差分、ε₁ ，ε₂ ，ε₃ …衝突判定値、Ｋpp…位置フィードバック比例ゲイン。

Claims (11)

1. 複数の関節からなるロボットのリンクやリンク先端に把持されたツールまたはワークが周辺の障害物等に衝突したとき、リンク等の変形やロボット可動部の損傷を回避すべく衝突したことを検出しかつ直ちにロボットを停止させる方法において、
   ロボットの各関節駆動用モータを制御するためのサーボドライバが検出したモータの位置・速度・加速度から、衝突が発生していない場合に各モータが発生すべきトルクを、ロボットの各関節の位置θ、速度θ_d 、加速度θ_ddとロボット機構パラメータとをもとにした
   で示す式に基づく逆動力学演算により予測し、
   該トルク予測値と実際の関節駆動トルクとの差分を求め、
   該差分の絶対値が予め設定された判定値より大きい場合に衝突が発生したものと見做し、
   複数の関節のうち停止もしくは低速で動作している関節あるいは慣性力が小さいため動作停止に必要なトルクが最大トルクと比較して十分小さい関節は、衝突があった後に衝突による干渉力を和らげるべく逃げ動作できるようにしたことを特徴とする多関節ロボットにおける衝突検出・停止制御法。
2. 衝突発生時の各関節の速度と負荷イナーシャからロボットの動作を停止させるために必要な最小限のトルク値を関節ごとに算出し、その最小限のトルク値をサーボドライバのトルク制限器に設定すると共に全てのサーボドライバの位置フィードバック比例ゲインを０とし、強制的に速度指令値を０とした速度フィードバック制御による減速処理を行わせるようにしたことを特徴とする請求項１に記載された多関節ロボットにおける衝突検出・停止制御法。
3. ロボットの動作を停止させるために必要な各関節のトルク値の算出においては、全ての関節が同じ時間で停止すると仮定し、関節ごとに減速停止時間を算出した後、全ての減速停止時間のうちの最大値をいずれの関節の減速停止時間にも充てるようにしたことを特徴とする請求項１または請求項２に記載された多関節ロボットにおける衝突検出・停止制御法。
4. 衝突が発生していない場合に各関節を駆動するモータが発生すべきトルクを予測するときに使用されるモータの位置・速度・加速度は、サーボドライバが検出したモータの位置・速度・加速度に代えて、サーボドライバに位置指令すべく予め教示されている作業データの再生動作により演算した関節位置と該関節位置をもとにして計算された速度および加速度であることを特徴とする請求項１ないし請求項３のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。
5. 前記逆動力学演算は、ロボットの関節数をｎとして、ロボットの各関節の加速度により各関節で発生するトルクならびに他の関節の加速度により発生する干渉トルクを示すｎ×ｎの慣性行列、遠心力とコリオリ力の影響を示すｎ×１の粘性行列、および重力加速度の影響により発生するトルクを示すｎ×３の重力行列を含むことを特徴とする請求項１ないし請求項４のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。
6. 前記ｎ×ｎの慣性行列における対角要素としての「関節の持つ加速度が該関節自体に及ぼすトルクの関係を示した慣性モーメント」には、減速機を含むモータロータの慣性モーメントが減速比の２乗倍して加算され、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮することができるようにしたことを特徴とする請求項５に記載された多関節ロボットにおける衝突検出・停止制御法。
7. 前記逆動力学演算に供される式に、
   ただし、Ｃｍ_i 関節ｉにおける減速機の動粘性係数
   （ｉ＝１，２，・・・ｎ 以下同じ）
   θ_d ｍ_i 関節ｉにおけるモータの速度応答
   Ｆcou _i 関節ｉにおける減速機のクーロン摩擦トルク
   sgn （θ_d ｍ_i ） 符号関数
   ＝ １ ； θ_d ｍ＞０のとき
   ＝ ０ ； θ_d ｍ＝０のとき
   ＝−１ ； θ_d ｍ＜０のとき
   が加算され、各関節内のモータ軸受や減速機の摩擦抵抗を考慮することができるようにしたことを特徴とする請求項１ないし請求項６のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。
8. 前記ロボットの機構パラメータは、各関節に連結されるリンクの重心位置・質量・長さであることを特徴とする請求項１ないし請求項７のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。
9. 前記ロボットの機構パラメータには、慣性テンソルも付加されていることを特徴とする請求項８に記載された多関節ロボットにおける衝突検出・停止制御法。
10. 前記実際の関節駆動トルクは、実際にサーボドライバが生成したトルク指令値から算出されるトルクであることを特徴とする請求項１ないし請求項９のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。
11. 前記実際の関節駆動トルクは、各モータにおけるトルク応答値から算出されるトルクであることを特徴とする請求項１ないし請求項９のいずれか一項に記載された多関節ロボットにおける衝突検出・停止制御法。