(57)【要約】      （修正有）【課題】モータトルクを用いてロボットの衝突検出をする場合、動力学モデルにおける摩擦トルクは個体差や環境による測定誤差が大きいため、従来、衝突検出閾値を大きくして誤差のマージンをとるか、摩擦力推定のオブザーバを追加していた。これら従来の方式に比べ、衝突検出閾値が精緻に設定でき、かつ、応答性に優れた衝突検知方法を提供する。【解決手段】フィードバック制御より得た電流指令に動作トルクを加算して制御することで、動力学トルクで補償しきれない摩擦誤差としてフィードバック電流指令を衝突検出閾値設定値に加算し、衝突検出閾値として用いる。【選択図】図１
2005279872.tif 000002
【特許請求の範囲】
【請求項１】
減速機を介してモータにより駆動されるロボットにおいて、ロボットの逆動力学演算で求めた動力学トルクをモータ発生トルクから差し引くことにより、衝突による外力を検出するロボットの制御方法であって、モータの駆動電流のフィードバック制御により得られた電流指令を摩擦トルクの誤差分として記憶し、これを衝突検出閾値の設定値に加算し、衝突検出閾値として用いて次サイクル運転時のモータ発生トルクにおける摩擦損失を算出し、衝突検出を行うロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、減速機を介してモータにより駆動されるロボットの制御方法に関する。
【背景技術】
【０００２】
近年、ロボットにおいて、衝突時の安全性向上や破壊による損失防止のために、衝突検出の高精度化が求められている。しかしながら、高精度な衝突センサを用いることはコストが増大し、さらにロボットのアームに設けたセンサの重量が負荷として働くので、ロボットの高速化や省エネに反することになる。そこで、センサを用いずに衝突力を高精度に検出することが求められている。
【０００３】
このような衝突力をセンサレスで求める方法としては、モータの駆動電流で発生したトルクからモータ及び減速機で損失するトルクを差し引いたモータ発生トルクから、ロボットの逆動力学演算により求めたロボットの動力学トルクを差し引いて衝突力を求める方法が一般的である。
【０００４】
しかし、モータ及び減速機における摩擦力（＝粘性摩擦＋動摩擦）は、個体差や環境（温度や経年変化等）による測定誤差が大きく、衝突検出閾値を大きくして誤差のマージンをとるか（検出感度の鈍化）、摩擦力推定を行うかのいずれかであった。摩擦力推定においては事前に特定の動作で推定する方法（例えば特許文献１参照）と実時間で状態推定オブザーバを追加して推定する方法（例えば特許文献２参照）がある。
【０００５】
以上の様に、モータ発生トルクからロボットの動力学計算により求めたロボットの駆動力を差し引いて、センサレスで衝突力を求める場合や、モータ駆動力を最大限に発揮させるために動力学トルクのフィードフォワード制御によるサーボ追従特性の改善する場合においては、モータ発生トルク及びロボットの動力学トルクを正確に計算することが求められる。この場合、ロボット動作時におけるモータ発生トルクτｍは、モータ駆動側から見れば（数１）、負荷側から見れば（数２）で表される。
【０００６】
【数１】
2005279872.tif 000003
【０００７】
【数２】
2005279872.tif 000004
【０００８】
モータとロボットアームは減速機を介して接続しているので、（数２）においてモータイナーシャＪ以外の項は、減速比を用いてモータ軸端に換算する必要がある。
【０００９】
衝突力τｄｉｓは、（数１）、（数２）においてτｍｍ＝τｍｌと仮定する事により、以下の（数３）に変形して求めることが出来る。
【００１０】
【数３】
2005279872.tif 000005
【００１１】
（数３）における動摩擦トルクτμに誤差が生じると、衝突トルクτｄｉｓに誤差が含まれることとなる。
【００１２】
そこで、動摩擦力τμの誤差へ対応する際、衝突したかどうかを判断する衝突トルク検出閾値を大きくして、動摩擦トルク誤差に対するマージンをとる方法では、衝突検出感度が鈍化し、衝突力緩和の効果も小さくなる。
【００１３】
また、摩擦トルク推定における事前に特定の動作で推定する方法（例えば特許文献３参照）では、動摩擦力τμが動力学トルクτｄｙｎと無関係で一定の場合は正しく推定できるのだが、実際のところ、動摩擦トルクτμは動力学トルクτｄｙｎに比例して変動するので正しく推定できない。
【００１４】
ロボットに用いられる代表的な減速機における動摩擦トルクτμを外乱トルクτｄｉｓ＝０の条件で動力学トルクτｄｙｎの変動に対し測定したものが、図６、７である。
【００１５】
図６はハーモニック減速機、図７はＲＶ減速機における特性である。
【００１６】
図６、図７とも、動力学トルクτｄｙｎの増大に比例して、動摩擦トルクτμが増大していることがわかり、（数４）で近似することが出来る。
【００１７】
【数４】
2005279872.tif 000006
【００１８】
（数４）において、Ａ、Ｂ、Ｃ、Ｄ、Ｅ、Ｆは近似定数、τｔｈは設定閾値である。
【００１９】
図６、図７における上記パラメータは表１になる。
【００２０】
【表１】
2005279872.tif 000007
【００２１】
すなわち、特定動作パターンによる摩擦トルク推定方法では、モータ発生トルクも特定パターンであり、時々刻々変化する実際のモータ発生トルクに対しては十分な精度で推定できないことがわかる。
【００２２】
また、実時間で状態推定オブザーバを追加して摩擦トルクτμを推定する方法（例えば特許文献４参照）もあるが、経時変化ではなく、加減速の短期間に大きく摩擦トルクτμが変動するので、推定アルゴリズムを用いた場合は、摩擦トルク推定に位相遅れが生じ、衝突トルクτｄｉｓの検出精度が劣化する可能性がある。
【特許文献１】特開平８−２５２７８７号公報
【特許文献２】特開平６−８３４０３号公報
【特許文献３】特開平８−２５２７８７号公報
【特許文献４】特開平６−８３４０３号公報
【発明の開示】
【発明が解決しようとする課題】
【００２３】
以上のように従来の衝突検出の方法では何れのものもそれぞれ問題があった。
【００２４】
本発明は、これらの問題点を解決するためになされたものであり、衝突トルク検出における動摩擦誤差を精度良く検出し、衝突検出精度の向上を可能とする制御方法の提供を目的とする。
【課題を解決するための手段】
【００２５】
上記課題を解決するために、減速機を介してモータにより駆動されるロボットにおいて、ロボットの逆動力学演算で求めた動力学トルクをモータ発生トルクから差し引くことにより、衝突による外力を検出するロボットの制御方法であって、モータの駆動電流のフィードバック制御により得られた電流指令を摩擦トルクの誤差分として記憶し、これを衝突検出閾値の設定値に加算し、衝突検出閾値として用いて次サイクル運転時のモータ発生トルクにおける摩擦損失を算出し、衝突検出を行うロボットの制御方法である。
【発明の効果】
【００２６】
以上のように、本発明のロボットの制御方法は、減速機を介してモータにより駆動されるロボットにおいて、モータ発生トルクから、ロボットの逆動力学演算で求めた動力学トルク及びを差し引くことにより、衝突による外力をセンサレスで検出するロボットの制御方法であって、フィードバック制御により得られた電流指令に動力学トルクをフィードフォワード加算してロボットを制御することにより、フィードバック電流指令を摩擦パラメータの誤差分として記憶し、次サイクル運転時のモータ発生トルクにおける摩擦損失計算の精度を上げることにより、衝突検出精度の向上を実現することができる。
【発明を実施するための最良の形態】
【００２７】
（実施の形態）
以下、本発明のロボットの制御方法における実施の形態例について、図面を参照しながら説明する。
なお、各図において同じ構成については同じ符号を付してその説明を省略する。
図１は、本実施の形態における制御方法を示すブロック線図である。
【００２８】
図１において、動力学トルクフィードフォワード（以下ＦＦと略す）補償（２）は、位置指令θｃｏｍ（１）を元に動力学トルクを演算することにより、ロボット動作に必要なトルクをモータ（８）に発生させるＦＦ電流指令ＩＦＦ（３）を演算する。
【００２９】
ただし、衝突トルク（１７）等の外乱や動力学演算におけるパラメータ誤差の影響で、このＦＦ電流指令ＩＦＦ（３）のみでは、モータ（８）の回転検出器（９）により得られた位置フィードバック（以下ＦＢと略す）θＦＢ（４）を位置指令θｃｏｍ（１）に追従させることは不可能である。
【００３０】
そこで、位置指令θｃｏｍ（１）と位置ＦＢθＦＢ（４）の偏差を無くすべく、ＦＢ制御器（５）で、例えば偏差のＰＩＤ演算を行うことにより、ＦＢ電流指令ＩＦＢ（６）を発生させる。これをＦＦ電流指令ＩＦＦ（３）に加算することにより、電流指令Ｉｃｏｍ（７）を発生させ、ＦＦ電流指令ＩＦＦ（３）の誤差を補正する。
【００３１】
この状態を説明したものが図２である。
【００３２】
図２においては、モータ（８）の位置指令θｃｏｍ（１）を差分して得られる回転速度ωｃｏｍ（２１）が示す通り、加速→一定速→減速の運転を示している。
【００３３】
動力学トルクＦＦ補償ブロック（２）はこの位置指令θｃｏｍ（１）（速度単位ではωｃｏｍ（２１））の動作パターンに従って、ＦＦ電流指令ＩＦＦ（３）を発生させる。
【００３４】
さて、図１において、ロボットが１サイクル目運転では、衝突検出閾値Ｉｈｒ（１２）は動摩擦トルク誤差に対するマージンを大きくとられた衝突検出閾値初期値Ｉｈ０（１０）を用いる。初運転において衝突検出閾値Ｉｈｒ（１２）は衝突検出閾値切り替えＳＷ（１１）によって、衝突検出閾値初期値Ｉｈ０（１０）が選択される。衝突検出閾値初期値Ｉｈ０（１０）には、通常と同じく摩擦の損失を考慮し誤差のマージンを大きくとった値を選択する。
【００３５】
１サイクル運転より次の運転における衝突検出方法を示すブロック線図が図３である。
【００３６】
１サイクル運転より次の運転では、位置指令θｃｏｍ（１）と位置ＦＢθＦＢ（４）の偏差を無くすべく、ＦＢ制御器（５）により作成されるＦＢ電流指令ＩＦＢ（６）は１サイクル目運転時におけてＦＦ電流指令ＩＦＦ（３）が補償しきれなかった摩擦力パラメータの誤差値として現れる。
【００３７】
つまり、ＦＢ電流指令ＩＦＢ（６）を記憶装置に記憶し、あらかじめ説定しておいた衝突検出閾値設定値Ｉｈｄ（１４）に加え、この加算された結果を衝突検出閾値Ｉｈｒ（１２）として用いることにより、誤差のマージンを大きくとられていた衝突検出閾値において摩擦の損失を含んだ値を設定することができる。
【００３８】
この状態を示したものが図４である。
【００３９】
図４において、図２と同様に回転速度ωｃｏｍ（２１）が示す通り、加速→一定速→減速の運転を示している。
【００４０】
そこで１サイクル運転より次の運転ではあらかじめ設定しておいた衝突検出閾値設定値Ｉｈｄ（１４）に、ＦＢ電流指令ＩＦＢ（６）を平均化したＦＢ電流指令ＩＦＢＡ（１６）を加えた値を衝突検出閾値Ｉｈｒ（１２）として用いることで摩擦損失を考慮し、衝突検出精度を向上させることができる。
【００４１】
ＦＢ電流指令ＩＦＢＡ（１６）は常に現サイクルのＦＢ電流指令ＩＦＢ（６）を加算・平均化（１５）し、サイクル毎に更新した値である。
【００４２】
衝突トルク（１７）が加わった場合には、その力をモータトルク定数Ｋｔ（１８）を用いて電流値への換算が行われ、これを補償すべく、ＦＢ制御器（５）はＦＢ電流指令ＩＦＢ（６）を増加させることになる。
したがって、ＦＢ電流指令ＩＦＢ（６）は衝突検出閾値設定値Ｉｈｄ（１４）より大きい値をとり、衝突を検出することが出来る。
【００４３】
この状態を示したものが図５である。
【００４４】
従来の技術では、衝突検出閾値設定値Ｉｈｄ（１４）は衝突検出閾値初期値Ｉｈ０のように摩擦の損失を考慮し誤差のマージンを大きくとった値が選択されているので、衝突検出のタイミングは衝突発生時間τ０（２２）に対して誤差のマージンにより衝突検出時間τ１（２３）となり遅れを生じることになる。
【００４５】
本実施の形態では衝突検出閾値Ｉｈｒ（１２）に、衝突検出閾値設定値Ｉｈｄ（１４）にＦＢ電流指令ＩＦＢ（６）を平均化したＦＢ電流指令ＩＦＢＡ（１６）を加えた値を用いるので、衝突検出のタイミングは衝突検出時間τ２（２４）となり、衝突発生時間τ０（２２）に対して衝突検出精度を向上させることができる。
【産業上の利用可能性】
【００４６】
本発明のロボットの制御方法によれば、衝突検出精度の向上を実現することができるので例えば生産に用いるロボットなどに有用である。
【図面の簡単な説明】
【００４７】
【図１】本発明の衝突トルク検出方法を示すブロック線図
【図２】ロボットの加速→一定速→減速という運転において、回転速度の変動に対する電流指令、ＦＦ電流指令、ＦＢ電流指令特性の一例を示す図
【図３】１サイクル運転より次の運転における衝突トルク検出方法のブロック線図
【図４】衝突検出閾値の特性の一例を示す図
【図５】衝突トルクが加わった場合におけるＦＢ電流指令値の一例を示す図
【図６】ハーモニック減速機における動摩擦トルク特性の一例を示す図
【図７】ＲＶ減速機におけ動摩擦トルク特性の一例を示す図
【符号の説明】
【００４８】
７  電流指令
８  モータ
１１  衝突検出閾値切り替えＳＷ
１２  衝突検出閾値
１３  衝突検出判定
【図１】
2005279872.tif 000008
【図２】
2005279872.tif 000009
【図３】
2005279872.tif 000010
【図４】
2005279872.tif 000011
【図５】
2005279872.tif 000012
【図６】
2005279872.tif 000013
【図７】
2005279872.tif 000014