(57)【要約】【課題】高精度な衝突検出を維持したまま衝突誤検出を防止するためには、周辺の設備等との衝突以外の要因によるモータ発生トルクの飽和時など、必要な場合のみ正確に衝突検出閾値を上昇させる必要がある。【解決手段】モータにより駆動されるロボットにおいて、モータが減速機に出力するトルクからロボットの逆動力学演算を用いてモータ発生トルクフィードバック推定値を算出する算出部で求めたモータ発生トルクフィードバック推定値を差し引くことにより衝突による外力を検出し、外力の検出値が予め設定した所定の閾値より大きければ外力を受けたと判断する衝突検出方法を有し、位置指令と速度指令と加速度指令に基づいて前記算出部により求めたモータ発生トルク指令予測値が予め設定した所定値より大きければ、衝突検出における閾値を前記所定の閾値よりも上げて衝突検出を行う。【選択図】図１
2013169609.tif 000002
【特許請求の範囲】
【請求項１】
減速機を介してモータにより駆動されるロボットにおいて、モータが前記減速機に出力するトルクからロボットの逆動力学演算を用いてモータ発生トルクフィードバック推定値を算出する算出部で求めたモータ発生トルクフィードバック推定値を差し引くことにより、衝突による外力を検出し、前記外力の検出値が予め設定した所定の閾値より大きければロボットを構成するアームが外力を受けたと判断する衝突検出方法を有し、
位置指令と速度指令と加速度指令に基づいて前記算出部により求めたモータ発生トルク指令予測値が、予め設定した所定値より大きければ、前記衝突検出における閾値を前記所定の閾値よりも上げて前記衝突検出方法を行うロボットの衝突検出方法。
【請求項２】
衝突検出に用いる閾値を上げた状態を所定時間維持し、前記所定時間後に上げる前の閾値に戻す請求項１記載のロボットの衝突検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、産業用ロボットの衝突検出方法に関する。
【背景技術】
【０００２】
近年、産業用ロボットにおいて、ロボットが周囲の設備等に衝突した際の安全性向上や相互破壊による損失防止のため、衝突検出機能の高精度化が求められている。しかしながら、そのために高精度な衝突力センサを用いることは、コストの増大、センサの頑健性やセンサ自身の重量負荷としての性質等の問題の発生を招き、ロボットのパフォーマンスを制限する一因となってしまう。従って、高精度かつセンサレスに衝突力を検出できる方法が最も要求度の高いものとなっている。
【０００３】
センサレスという条件化において、衝突力を高精度に求める方法としては、モータの駆動電流で発生したトルクからモータ及び減速機のイナーシャと摩擦で損失するトルクを差し引いた減速機出力トルクより、ロボットの逆動力学演算から求めたロボットの動力学トルクを差し引いて衝突力を求める方式（以下、動力学演算方式という）がある（例えば、特許文献１参照）。
【０００４】
図４は、動力学演算方式の制御ブロック線図である。図４に示す位置制御器２０において、位置指令の速度成分ｄθｃｏｍを積分した位置指令θｃｏｍと、モータ速度フィードバックωｍを積分したモータ位置フィードバックθｍとの差分値から速度ループ指令ωｃｏｍを生成する。
【０００５】
速度制御器２１において、速度ループ指令ωｃｏｍとモータ速度フィードバックωｍの差分値からモータ電流Ｉｍを生成する。
【０００６】
また、τｍはモータ発生トルクであり、減速機が剛体であると仮定すると、モータ発生トルクτｍは、以下に示す（数１）において、モータ駆動側から見れば（数１−１）で表され、負荷側から見れば（数１−２）で表される。
【０００７】
【数１】
2013169609.tif 000003
【０００８】
ここで、
Ｋｔ    ：モータトルク定数
Ｉｍ    ：モータ電流
αｍ    ：モータ加速度フィードバック
ωｍ    ：モータ速度フィードバック
Ｊｍ    ：モータイナーシャ（ロータ＋減速機１次側）
Ｄ      ：粘性摩擦抵抗
τμ    ：動摩擦トルク
τｄｙｎ：動力学トルク（重力トルク、慣性力、遠心力、コリオリ力の和）
τｄｉｓ：衝突トルク
また、上記に示す動摩擦トルクτμは、以下に示す（数２）で計算できる。
【０００９】
【数２】
2013169609.tif 000004
【００１０】
ここで、
Ｋμ：動摩擦の大きさ
１（ωｍ＞０）
ｓｇｎ  ＝    ０（ωｍ＝０）
−１（ωｍ＜０）
また、（数１−２）の右辺にある衝突トルクτｄｉｓは、（数１−１）と（数１−２）より、以下に示す（数３）に変形して求めることが出来る。
【００１１】
【数３】
2013169609.tif 000005
【００１２】
なお、上記（数３）において、Ｋｔ×Ｉｍ−Ｊｍ×αｍ−Ｄ×ωｍ−Ｋμ×ｓｇｎはモータが減速機に出力するトルクであり、τｄｙｎは動力学トルクである。
【００１３】
図４に破線で示す動力学演算モジュールブロック３０において、動力学トルク推定値τｄｙｎｏは、逆動力学演算ブロック２７において、ロボットを構成する全軸のモータ速度フィードバックωｍとロボットの機械パラメータを用いて逆動力学演算を実行することで求められる。動力学演算モジュールブロック３０は、この動力学トルク推定値τｄｙｎｏと動摩擦トルク推定値τμｏとモータ伝達関数のインバースのブロック２４で算出されるモータ単体の慣性と摩擦により発生するトルクとの加算値（以下、モータ発生トルクフィードバック推定値という）を求める。そして、モータ発生トルクフィードバック推定値とモータ発生トルクτｍの差分を取ることで衝突トルク推定値τｄｉｓｏを求め、この衝突トルク推定値τｄｉｓｏを衝突検出判定ブロック２８へ出力する。
【００１４】
衝突検出判定ブロック２８は、所定の衝突検出閾値τｔｈを用いて、以下に示す（数４）に従い衝突を検出する。
【００１５】
【数４】
2013169609.tif 000006
【００１６】
上記した動力学演算方式は、高精度な反面、それが仇となる場合がある。高精度ということは衝突検出が非常に敏感であるということである。つまり、動力学演算方式により求められたモータ発生トルクフィードバック推定値とモータ発生トルクとの差がそれほど大きくない（ロボットが何所にも衝突していない）場合でも、衝突が検出されてしまう可能性がある。
【００１７】
そのため、ロボットの操作者が意図しない動作点において、衝突検出すなわち誤検出を起こしてしまう場合が大いに考えられる。これを防止するためには、衝突検出閾値を上げて衝突検出の感度を鈍らせるといった方法が考えられる。しかし、それでは本末転倒といえる。そこで、モータ発生トルクフィードバック推定値とモータ発生トルクの間にずれが出やすい、例えば急加速等の動作においてのみ、衝突検出閾値を上昇させる方法が求められる。
【００１８】
そのような衝突誤検出防止の方法として、特定の条件化において、衝突検出閾値を上昇させる方法が挙げられる。例えば、特許文献１には、加速度指令に基づいて衝突検出閾値を上昇させる方法が記載されている。この方法は、加速度指令が予め設定された閾値以上となった場合に、衝突検出閾値を通常よりも高い値に再設定している。
【００１９】
また、低温環境下におけるロボット駆動軸の摩擦力が増大した場合の衝突誤検出防止の方法として、駆動軸の温度を基にして衝突検出閾値を上昇させる方法が記載されている（例えば、特許文献２参照）。この方法では、温度センサ等により駆動軸の温度を検出し、それが予め設定された閾値以下となった場合に、衝突検出閾値を通常よりも高い値に再設定している。
【先行技術文献】
【特許文献】
【００２０】
【特許文献１】特開２００６−１１６６５０号公報
【特許文献２】特開平１１−１５５１１号公報
【発明の概要】
【発明が解決しようとする課題】
【００２１】
特許文献１に記載の方法は、加速度指令に基づいて衝突検出閾値を上昇させるものであり、実際に検出されている衝突トルクの値に関係なく衝突検出閾値を上昇させることになり、必要の無い動作点においても衝突検出閾値を上昇させてしまう場合がある。従って、結果的に衝突検出の精度を自ら落としていることになる。このようなケースは、実際のロボットの動作において多々存在する。
【００２２】
ここで、衝突誤検出は、モータ電流すなわちモータ発生トルクが、安全のためにモータ仕様毎に決められているリミット値の付近になる場合に起こりやすい。図５にその概略を示す。
【００２３】
図５は、実際にモータに流れている電流に起因するモータ発生トルク３１と、予め設定されているモータ発生トルク３１のリミット値３２と、動力学演算モジュールブロック３０で求められたモータ発生トルクフィードバック推定値３３と、衝突検出閾値３４と、衝突トルク３５を示している。
【００２４】
図５において、モータ発生トルク３１は、予め設定されたリミット値３２により制限される。しかし、動力学演算モジュールブロック３０で求められたモータ発生トルクフィードバック推定値３３にはリミット値が存在せず、リミット値３２以上の値となっている。そのため、衝突検出閾値３４を超える衝突トルク３５が発生し、衝突誤検出が起こってしまう。
【００２５】
また、周囲の設備等に衝突した際にもモータ発生トルクは飽和することがある。そのため、その場合のことを考慮すると、モータ発生トルクフィードバック推定値にリミット値を設けることができないことをここに述べておく。
【００２６】
これらのことから、衝突誤検出防止機能としては、必要な場合のみ正確に衝突検出閾値を上昇させることで、高精度な衝突検出を維持したまま衝突誤検出を防止できるようなものが望ましいといえる。
【００２７】
本発明は、指令値（位置指令、速度指令、加速度指令）と動力学演算モジュールブロックを用い、上記を満たす衝突誤検出機能を有するロボットの衝突検出方法を提供することを目的とする。
【課題を解決するための手段】
【００２８】
上記課題を解決するために、本発明のロボットの衝突検出方法は、減速機を介してモータにより駆動されるロボットにおいて、モータが前記減速機に出力するトルクからロボットの逆動力学演算を用いてモータ発生トルクフィードバック推定値を算出する算出部で求めたモータ発生トルクフィードバック推定値を差し引くことにより、衝突による外力を検出し、前記外力の検出値が予め設定した所定の閾値より大きければロボットを構成するアームが外力を受けたと判断する衝突検出方法を有し、位置指令と速度指令と加速度指令に基づいて前記算出部により求めたモータ発生トルク指令予測値が、予め設定した所定値より大きければ、前記衝突検出における閾値を前記所定の閾値よりも上げて前記衝突検出方法を行うものである。
【００２９】
また、本発明のロボットの衝突検出方法は、上記に加えて、衝突検出に用いる閾値を上げた状態を所定時間維持し、前記所定時間後に上げる前の閾値に戻すものである。
【発明の効果】
【００３０】
以上のように、本発明によれば、指令値（位置指令、速度指令、加速度指令）と動力学演算を行う算出部を用いて衝突検出の閾値を変更する衝突誤検出防止機能を行うことにより、高精度な衝突検出を実現することができる。
【図面の簡単な説明】
【００３１】
【図１】本発明の実施の形態１における衝突誤検出防止機能を実現する構成の概略を示す図
【図２】本発明の実施の形態１における衝突誤検出防止機能の動作を説明する図
【図３】本発明の実施の形態１における衝突検出機能（動力学演算方式）の制御ブロック線図
【図４】従来の衝突検出機能（動力学演算方式）の制御ブロック線図
【図５】従来の衝突検出機能における衝突誤検出の発生を説明する図
【発明を実施するための形態】
【００３２】
（実施の形態１）
本実施の形態１について、図１から図３を用いて説明する。図１は、本実施の形態１における衝突誤検出防止機能を実行するための構成を示す図である。図２は、本実施の形態１における衝突誤検出防止機能の実際の動作を説明するための図である。図３は、本発明の実施の形態１における衝突検出機能（動力学演算方式）の制御ブロック線図である。
【００３３】
なお、本実施の形態１において、背景技術の項で説明した図４と同様の箇所については、同一の符号を付して詳細な説明を省略する。
【００３４】
図１において、位置指令設定部１と、速度指令設定部２と、加速度指令設定部３と、動力学演算によりモータ発生トルク指令予測値を算出する演算部４と、衝突誤検出防止機能実行判定閾値設定部５と、衝突誤検出防止機能実行判定部６と、処理切替のスイッチ７と、通常用衝突検出閾値設定部８と、衝突検出閾値上昇部９と、上昇保持時間設定部１０と、衝突誤検出防止用衝突検出閾値設定部１１を示している。なお、これらの構成要素は、例えば、多関節型ロボットを構成するマニピュレータとロボット制御装置の内、ロボット制御装置の内部に設けられている。また、演算部４は、図３に示す動力学演算モジュールブロック３０に相当する。
【００３５】
図１の動作について説明する。位置指令設定部１では、ロボット制御装置内に記憶された動作プログラム内の命令であり、教示により与えられた距離と動作速度を含む動作命令に基づいて位置指令を設定する。速度指令設定部２では、位置指令設定部１の場合と同様に、動作命令に基づいて速度指令を設定する。加速度指令設定部３では、位置指令設定部１の場合と同様に、動作命令に基づいて加速度指令を設定する。
【００３６】
演算部４では、設定された位置指令と速度指令と加速度指令と他軸の速度指令を入力とし、これらに基づいてモータ発生トルク指令予測値を算出する。なお、衝突を検出するために、図４を用いて説明したように、図３においてモータ発生トルクフィードバック推定値を算出する場合には、自軸と他軸のモータ速度フィードバックωｍを用いて算出を行う。しかし、後述する本実施の形態１のおける衝突誤検出防止機能を行うためにモータ発生トルク指令予測値を算出する場合には、自軸と他軸のモータ速度フィードバックωｍは用いず、自軸と他軸の速度指令を用いて算出を行う。また、演算部４では、速度指令から位置指令と加速度指令を求めて算出に使用している。すなわち、位置指令と加速度指令は、動作命令の１つである速度指令に基づいて設定される。
【００３７】
衝突誤検出防止機能実行判定閾値設定部５では、予めロボット制御装置の内部に記憶された衝突誤検出防止機能を実行するか否かを判定するための閾値を設定する。衝突誤検出防止機能実行判定部６では、演算部４で算出したモータ発生トルク指令予測値と、衝突誤検出防止機能実行判定閾値設定部５で設定された閾値とを比較し、衝突誤検出防止機能を実行するか否かを判定する。モータ発生トルク指令予測値≧閾値の場合、「衝突誤検出防止機能を実行する」と判定される。また、モータ発生トルク指令予測値＜閾値の場合、「衝突誤検出防止機能を実行しない」と判定される。なお、衝突誤検出防止機能の実行判定用の閾値は、実験的および経験則により、モータ発生トルクのリミット値よりも低い値として設定される。
【００３８】
衝突誤検出防止機能実行判定部６において「衝突誤検出防止機能を実行しない」と判定された場合、衝突誤検出防止機能実行判定部６と通常用衝突検出閾値設定部８とが接続されるようにスイッチ７が動作する。そして、通常用衝突検出閾値設定部８において、予めロボット制御装置の内部に記憶された衝突検出の閾値が、衝突検出閾値として設定される。すなわち、衝突検出閾値が設定される。
【００３９】
一方、衝突誤検出防止機能実行判定部６において「衝突誤検出防止機能を実行する」と判定された場合、衝突誤検出防止機能実行判定部６が衝突検出閾値上昇部９に接続されるようにスイッチ７が動作する。そして、衝突検出閾値上昇部９において、衝突検出閾値が、予めロボットの制御装置内部に記憶されていた通常用衝突検出閾値設定部８で設定されたる閾値よりも更に高い値であり、予めロボット制御装置の内部に記憶されていた衝突誤検出防止用衝突検出閾値に修正される。さらに、上昇保持時間設定部１０において、予めロボット制御装置の内部に記憶されていた上昇保持時間により、衝突検出閾値を上昇させる時間が設定される。そして、衝突検出閾値上昇部９の出力と上昇保持時間設定部１０の出力に基づいて、衝突誤検出防止用衝突検出閾値設定部１１において、衝突誤検出防止用衝突検出閾値として設定される。すなわち、衝突誤検出防止用衝突検出閾値とこの衝突誤検出防止用衝突検出閾値を維持する時間である上昇保持時間が設定される。
【００４０】
なお、衝突誤検出防止用衝突検出閾値および衝突検出閾値の上昇保持時間は、例えば実験的および経験則により決定されるものである。
【００４１】
次に、図２を用いて、衝突誤検出防止機能実行時の挙動について説明する。図２において、実際にモータに流れている電流に起因するモータ発生トルク１２と、予め設定されているモータ発生トルク１２のリミット値１３と、演算部４で求められたモータ発生トルク指令予測値１４と、衝突誤検出防止機能を実行するか否かを判定するための閾値１５と、衝突検出閾値１６と、衝突トルク１７と、衝突検出閾値の上昇保持時間１８を示している。
【００４２】
モータ発生トルク指令予測値１４が閾値１５を超えた場合、衝突誤検出防止機能により、衝突検出閾値１６が上昇保持時間１８の間だけ通常より高い値である衝突誤検出防止用衝突検出閾値に変更され、衝突検出閾値が上昇される。
【００４３】
そして、衝突検出閾値が上昇された状態で、背景技術で説明した図４と同様に、図３において、衝突の検出が行われる。
【００４４】
なお、ロボット制御装置において、所定のタイミングで衝突誤検出防止機能の実行の可否とそれに伴う衝突検出閾値の設定が行われ、また、所定のタイミングで衝突検出が行われ、これが繰り返されている。また、衝突誤検出防止機能の実行の可否に伴う衝突検出閾値の設定が行なわれた後に、この設定された衝突検出閾値に基づいて衝突検出を行うようになっている。
【００４５】
また、本実施の形態１のロボットの衝突検出方法によれば、背景技術で説明した特許文献１のように、指令値のみに基づいて衝突検出閾値を変更させるものではなく、指令値および実際に検出されている自軸と他軸のモータ速度フィードバックωｍを用いてモータ発生トルク指令予測値を算出し、衝突検出閾値を変更させるものである。従って、特許文献１に記載の方法のように、必要の無い動作点においても衝突検出閾値を上昇させてしまうことを防ぎ、特許文献１の方法の場合よりも精度よく衝突検出を行うことができる。
【００４６】
また、衝突誤検出防止機能の実行の可否の判定に用いるモータ発生トルク指令予測値の算出は、衝突検出に用いるモータ発生トルクフィードバック推定値を算出する演算部４において行われる。すなわち、演算部４を共用することにより行われる。従って、モータ発生トルク指令予測値を算出するための新たな算出部を設ける必要がない。
【産業上の利用可能性】
【００４７】
本発明のロボットの衝突検出方法によれば、衝突検出精度の向上を実現することができるので、例えば生産に用いる産業用ロボットなどで用いられるロボットの衝突検出方法として産業上有用である。
【符号の説明】
【００４８】
１    位置指令設定部
２    速度指令設定部
３    加速度指令設定部
４    演算部
５    衝突誤検出防止機能実行判定閾値設定部
６    衝突誤検出防止機能実行判定部
７    スイッチ
８    通常用衝突検出閾値設定部
９    衝突検出閾値上昇部
１０  上昇保持時間設定部
１１  衝突誤検出防止用衝突検出閾値設定部
１２  モータ発生トルク
１３  リミット値
１４  モータ発生トルク指令予測値
１５  閾値
１６  衝突検出閾値
１７  衝突トルク
１８  上昇保持時間
１９  積分器
２０  位置制御器
２１  速度制御器
２２  モータトルク定数
２３  モータ伝達関数ブロック
２４  モータ伝達関数のインバースのブロック
２５  モータ回転方向判定ブロック
２６  動摩擦係数
２７  逆動力学演算ブロック
２８  衝突検出判定ブロック
２９  モータ及び外力を示すブロック
３０  動力学演算モジュールブロック
３１  モータ発生トルク
３２  リミット値
３３  モータ発生トルクフィードバック推定値
３４  衝突検出閾値
３５  衝突トルク
【図１】
2013169609.tif 000007
【図２】
2013169609.tif 000008
【図３】
2013169609.tif 000009
【図４】
2013169609.tif 000010
【図５】
2013169609.tif 000011