JPWO2017047009A1 - ロボットの衝突検出方法 - Google Patents

Abstract

ロボットの衝突は以下の方法で検出する。ロボットは、モータと、モータに接続された減速機と、モータの回転を検出するエンコーダと、エンコーダに内蔵された温度センサと、減速機を介してモータにより駆動される対象物とを有する。モータが減速機に出力するトルクから、ロボットの逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値として推定する。衝突トルク推定値が所定の衝突検出閾値より大きければ、ロボットが外力を受けたと判断する。温度センサで検出された温度が所定の温度閾値未満である場合に、所定の衝突検出閾値を第１の値にする。検出された温度が所定の温度閾値以上である第１の時点より所定時間だけ前の第２の時点から第１の時点までの期間での衝突トルク推定値の最大値が第１の最大値判定閾値より小さい場合に、第１の時点において所定の衝突検出閾値を第１の値より小さい第２の値にする。上記期間での衝突トルク推定値の最大値が第１の最大値判定閾値以上である場合に、第１の時点において所定の衝突検出閾値を第１の値にする。

Description

本発明は、減速機を介してモータにより駆動されるロボットの衝突検出方法に関する。

近年、ロボットにおいて、衝突時の安全性向上や破壊による損失防止のために、衝突検出の高精度化が求められている。しかしながら、高精度な衝突センサを用いることはコストが増大し、さらに重量負荷としてのその衝突センサを振り回すことになるので、ロボットの高速化や省エネルギーに反することになる。そこで、衝突センサを用いないで、衝突力を高精度に求めることが望まれている。

衝突の際の衝突力を、衝突センサを用いないで求める方法の一例としては動力学演算方式が挙げられる（非特許文献１参照）。動力学演算方式では、モータの駆動電流で発生したトルクからモータ及び減速機のイナーシャと摩擦で損失するトルクとを差し引いた減速機出力トルクを求める。求めた減速機出力トルクより、ロボットの逆動力学演算から求めたロボットの動力学トルクを差し引いて衝突力を求める。

図１は、垂直多関節のロボットの位置制御に関する従来の構成を示すブロック図であり、ロボット機構１６１とロボット制御装置１６２の内部構成の概略を示す。ここで、ロボット機構１６１は複数のアームおよび関節軸を有しており、各関節軸の近傍に設けられた減速機を用いてロボットのアームおよび関節軸が駆動される。

図１において、ロボット制御装置１６２の内部に設けられたメイン制御部６４は、操作・教示部６３で指示され記憶されたロボット機構１６１のアーム等の移動の軌跡に従い、ロボットの各軸（例えば６軸）の位置指令θｃｏｍ（θｃｏｍ１〜θｃｏｍ６）を出力する。そして、位置指令θｃｏｍ（θｃｏｍ１〜θｃｏｍ６）にそれぞれ追従するように、ロボットの各軸のサーボ制御部１６５（１６５１〜１６５６）がロボット機構１６１内のモータ６６（６６１〜６６６）をそれぞれ制御する。

図２は、図１のサーボ制御部１６５を詳細に示したブロック線図であり、動力学演算方式の衝突センサを用いないセンサレス衝突検出機能を加えている。

図２において、位置制御ブロック６は、位置指令θｃｏｍとモータ位置フィードバックθｍとの差分値から、速度ループ指令ωｃｏｍを生成する。モータ位置フィードバックθｍはモータ６６に取り付けられた位置検出器であるエンコーダ５１から得られる。

図２の速度制御ブロック１０は、モータ位置フィードバックθｍを微分して得られるモータ速度フィードバックωｍと速度ループ指令ωｃｏｍとの差分値からモータ電流指令Ｉｍを生成する。

図２のブロック１８はモータ６６と外力を示す。減速機５３が剛体であると仮定すると、モータトルク定数Ｋｔと、モータ電流Ｉｍと、モータ角加速度αｍと、モータ角速度ωｍと、モータイナーシャＪｍと、粘性摩擦係数Ｄと、動摩擦トルクτμと、動力学トルクτｄｙｎと、衝突トルクτｄｉｓとにより、モータ発生トルクτｍは（数１）で表される。

なお、モータ角加速度αｍはモータ角速度ωｍの微分値である。モータイナーシャＪｍは、モータ６６のロータと減速機５３の一次側の和である。動力学トルクτｄｙｎは、重力トルクと慣性力と遠心力とコリオリ力の和である。

具体的には、モータ発生トルクτｍはモータ６６の駆動側から見れば（数１）のうちの第１の式で表され、モータ６６に対して減速機５３を介した負荷側から見れば（数１）の第２の式で表される。

上記の動摩擦トルクτμは、動摩擦係数Ｋμにより（数２）で計算できる。

また、（数１）の第２の式の右辺にある衝突トルクτｄｉｓは、（数１）より、以下に示す（数３）に変形して求めることが出来る。

なお、（数３）において、Ｋｔ×Ｉｍ−Ｊｍ×αｍ−Ｄ×ωｍ−Ｋμ×ｓｇｎはモータ６６が減速機５３に出力するトルクである。

図２において、（数３）はトルク推定ブロック３０として表される。

トルク推定ブロック３０において、動力学トルク推定値τｄｙｎｏは、トルク演算ブロック２６において、ロボットを構成する全軸のモータ速度フィードバックとロボットの機械パラメータを用いて逆動力学演算を実行することで求められる。トルク推定ブロック３０は、動力学トルク推定値τｄｙｎｏを用いて衝突トルク推定値τｄｉｓｏをもとめ、衝突トルク推定値τｄｉｓｏを衝突判定ブロック３１へ出力する。

衝突判定ブロック３１は、所定の衝突検出閾値τｔｈを用いて、（数４）に従い衝突を検出する。

すなわち、（数４）が成り立つ場合にメイン制御部６４は衝突が発生したと判定し、（数４）が成り立たない場合にはメイン制御部６４は衝突は発生していないと判定する。

図２の衝突トルク推定値τｄｉｓｏを求めるトルク推定ブロック３０において、パラメータ誤差や動力学トルク演算のモデル誤差等により、実際に衝突していない場合でも衝突トルク推定値τｄｉｓｏは「０」にはならない。つまり、実際に衝突していない場合は、衝突トルクの推定誤差が衝突トルク推定値τｄｉｓｏとして出力される。

パラメータ誤差の中でも、特に粘性摩擦係数Ｄは約５℃以下の低温になるとグリスの粘性が上昇して大きな誤差が発生する。通常温度で測定した粘性摩擦係数Ｄを用いて衝突トルク推定値τｄｉｓｏを求めると誤差が大きくなるので、衝突誤検出を防ぐために衝突検出閾値τｔｈを大きくしなければならない可能性がある。

特許文献１では、以下に説明するように温度を測定し、低温時に衝突検出閾値を引上げる方法が示されている。

図３、図４は、他の従来技術を示し、それぞれ図１、図２に対応し、減速機に温度センサを取り付け、衝突検出閾値を切換える構成が加えられている。

図３において、温度センサ５６が減速機５３に取り付けられている。温度センサ５６の温度センサ出力値Ｔｃを、第１のサーボ制御部１６５に入力している。

図４において、閾値切換ブロック３４では、温度センサ出力値Ｔｃを所定の温度閾値Ｔｃｔｈと比較し、常温時の衝突検出閾値τｔｈと低温に対応した閾値増分ｄτｔｈを用いて（数５）に従って衝突検出閾値τｖｔｈを衝突判定ブロック４０に出力する（ｄτｔｈ＞０）。

なお、常温時の衝突検出閾値τｔｈは、減速機５３の温度が常温（例えば５℃以上）で実際に動作させ予め求められている。また、低温に対応した閾値増分ｄτｔｈは低温（例えば５℃未満の温度）で実際に動作させて予め求められている。

衝突判定ブロック４０は、（数５）で演算された衝突検出閾値τｖｔｈを用いて、以下に示す（数６）に従い衝突を検出する。

すなわち、（数６）が成り立つ場合にメイン制御部６４は衝突が発生したと判定し、（数６）が成り立たない場合にはメイン制御部６４は衝突は発生していないと判定する。

特開平１１−１５５１１号公報

小菅一弘、他一名、"マニピュレータの動的衝突検出"、日本機械学会［Ｎｏ．９９‐９］ロボティクス・メカトロニクス講演会‘９９講演論文集 ２Ａ１‐１１‐０３０

ロボットの衝突は以下の方法で検出する。ロボットは、モータと、モータに接続された減速機と、モータの回転を検出するエンコーダと、エンコーダに内蔵された温度センサと、減速機を介してモータにより駆動される対象物とを有する。モータが減速機に出力するトルクから、ロボットの逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値として推定する。衝突トルク推定値が所定の衝突検出閾値より大きければ、ロボットが外力を受けたと判断する。温度センサで検出された温度が所定の温度閾値未満である場合に、所定の衝突検出閾値を第１の値にする。検出された温度が所定の温度閾値以上である第１の時点より所定時間だけ前の第２の時点から第１の時点までの期間での衝突トルク推定値の最大値が第１の最大値判定閾値より小さい場合に、第１の時点において所定の衝突検出閾値を第１の値より小さい第２の値にする。上記期間での衝突トルク推定値の最大値が第１の最大値判定閾値以上である場合に、第１の時点において所定の衝突検出閾値を第１の値にする。

この衝突検出方法は、エンコーダ内蔵の温度センサを用いて衝突検出閾値を切換える場合でも、衝突の誤検出の発生を減じることができる。

図１はロボットの位置制御に関する従来の構成を示すブロック図である。 図２は図１に示す構成での衝突検出方法を示すブロック線図である。 図３はロボットの位置制御に関する他の従来の構成を示すブロック図である。 図４は図３に示す構成での衝突検出方法を示すブロック線図である。 図５は垂直多関節ロボットの位置制御に関するさらに他の従来の構成を示すブロック図である。 図６は減速機にエンコーダ付モータが直接取り付けられた場合の位置関係を示す図である。 図７は減速機とエンコーダ付モータとの間にベルトが介在する場合の位置関係を示す図である。 図８は実施の形態におけるロボットの概略構成図である。 図９は実施の形態における衝突検出方法を示すブロック線図である。 図１０は実施の形態における波形分析ブロックの詳細を示すブロック図である。 図１１は実施の形態における最大値抽出ブロックの処理を示すフローチャートである。 図１２は実施の形態における衝突検出方法での波形を示す図である。 図１３は実施の形態における切換え許可判定ブロックの処理を示すフローチャートである。 図１４は実施の形態における閾値切換ブロックの処理を示すフローチャートである。 図１５は実施の形態における衝突検出方法での動作を示す図である。 図１６は実施の形態における衝突検出方法での他の動作を示す図である。 図１７は実施の形態における衝突検出方法でのさらに他の動作を示す図である。 図１８は図１７に示す動作を示す図である。 図１９は実施の形態における衝突検出方法でのさらに他の動作を示す図である。 図２０は図１９に示す動作を示す図である。

図３に示す構成で減速機５３の温度を測定するためには、温度センサ５６を減速機５３に取り付ける必要があるが、コストが上昇してしまう。また、温度センサ出力値Ｔｃをロボット制御装置１６２内のサーボ制御部１６５に入力するためには、ロボット機構１６１内のケーブルを増やす必要があり、配線処理や断線等の信頼性に問題が生じる可能性がある。

近年、モータに内蔵または取り付けられているモータの位置検出器であるエンコーダに温度センサが内蔵され、位置情報と同様にシリアル通信データで温度の読取りが可能となっている場合がある。

モータ動作発熱に起因してエンコーダ内温度が上昇し、約９０℃以上になると、エンコーダを構成する電子部品が誤動作し、正しい位置情報を送信できなくなる。そのため、温度センサを内蔵し、温度が設定値（約９０℃）以上になるとアラーム信号をシリアル信号で出力する。

図５は、図１に示すエンコーダ５１（５１１〜５１６）を、温度センサ５６（５６１〜５６６）をそれぞれ内蔵したエンコーダ５２（５２１〜５２６）に置き換えた構成を示している。モータ６６（６６１〜６６６）は減速機５３（５３１〜５３６）を介して対象物である関節軸６７Ａ（６７１Ａ〜６７６Ａ）をそれぞれ駆動する。エンコーダ５２に内蔵された温度センサ５６の温度センサ出力値Ｔｃは、位置情報と同じシリアル通信で読み取れるので、コストの上昇やロボット内のケーブルの数は増加しない。

ただし、この温度はモータ６６のエンコーダ５１の内部の温度であり、減速機の温度ではない。図６は減速機５３にエンコーダ５１が付されたモータ６６が直接取り付けられた場合の位置関係を示す。図７はエンコーダ５１が付されたモータ６６と減速機５３との間にベルト６８が介在する場合の位置関係を示す。図６に示す様に減速機５３がモータ６６と直結されている場合には、エンコーダ５１の内部温度と同程度に減速機５３の温度が上昇していると考えられる。しかし、図７に示す様にモータ６６と減速機５３の間にベルト６８等が介在する場合には、エンコーダ５２の内部温度と同程度に減速機５３の温度が上昇しているとは限らない。

モータ６６に内蔵または取り付けられているエンコーダ５２の内部温度が所定の温度閾値Ｔｃｔｈを超えた時、（数５）に従って衝突検出閾値τｖｔｈを演算し、低温に対応した閾値増分ｄτｔｈを無くして検出閾値τｖｔｈを元の値である低温時を考慮していない常温に対応する衝突検出閾値τｔｈに戻す。この時、モータ６６の駆動力が伝達される減速機５３の温度上昇が十分でなければ、粘性摩擦係数Ｄが高いことに起因する衝突トルク推定値τｄｉｓｏの誤差が衝突検出閾値τｖｔｈより大きくなり、衝突誤検出が発生する可能性がある。

（実施の形態）
図８は、垂直多関節六軸ロボットであるロボット１００１の概略構成図である。ロボット１００１はロボット機構６１とロボット制御装置６２とから構成される。ロボット機構６１は、複数のアーム６７（および複数の関節軸６７Ａ（６７１Ａ〜６７６Ａ）を有しており、各関節軸の近傍に設けられた減速機を用いて駆動される。

図９は、実施の形態における衝突検出方法を示すブロック線図である。図９は、図４で示した動力学演算方式をベースに、波形分析ブロック５７をさらに備える。

図１０は波形分析ブロック５７の詳細を示す。なお、以下の説明において、単位時間とは衝突トルク推定値の波形分析を行う間隔と定義する。実施の形態においてはこの単位時間は、例えば１分間である。なお、単位時間はロボット１００１が停止している時間は除き、稼動している時間を指す。

ピークホールドブロック７４は、衝突トルク推定値τｄｉｓｏの絶対値の単位時間内でのピークをホールドして得た値であるピークホールド値τｄＰを算出する。

複数の関節軸６７Ａ（６７１Ａ〜６７６Ａ）のうちの関節軸６７１Ａを駆動するモータ６６１と減速機５３１とエンコーダ５２と温度センサ５６１の動作について説明する。図１１はピークホールド値τｄＰの算出方法を示すフローチャートである。メイン制御部６４は、図１１の処理を衝突トルク推定値τｄｉｓｏの単位時間内において演算周期毎に所定の回数だけ実施する。演算周期は実施の形態では数ｍｓｅｃである。メイン制御部６４は単位時間内に図１１に示す動作を所定の回数ｎｕｍだけ行ってピークホールド値τｄＰを算出して出力する。

ピークホールドブロック７４は、ピークホールドの回数ｉが０であるか否かを判定する（ステップ１１−１）。ステップ１１−１において回数ｉが０すなわち初回であれば（ステップ１１−１の「Ｙｅｓ」）、ピークホールドブロック７４はピークホールド値τｄＰをクリアして０にする（ステップ１１−１Ａ）。メイン制御部６４は、ステップ１１−１において回数ｉが０ではないすなわち初回でなれば（ステップ１１−１の「Ｎｏ」）、メイン制御部６４はピークホールド値τｄＰをそのままにする。

ピークホールドブロック７４は、関節軸６７１Ａの角速度ωｍと他の関節軸６７２Ａ〜６７６Ａの角速度ωｍのうちの最大の絶対値を有する角速度ωｍＭＡＸの絶対値が角速度閾値ωｍｔｈより大きいか否かを判定する（ステップ１１−２）。角速度閾値ωｍｔｈはピークホールドを実行するか否かを判定する閾値である。ピークホールドブロック７４は、ステップ１１−２において角速度ωｍＭＡＸの絶対値が角速度閾値ωｍｔｈより大きい場合は（ステップ１１−２の「Ｙｅｓ」）、ピークホールドの回数ｉを１だけ増加させる（ステップ１１−２Ａ）。ピークホールドブロック７４は、ステップ１１−３において角速度ωｍＭＡＸの絶対値が角速度閾値ωｍｔｈより大きくない場合は（ステップ１１−２の「Ｎｏ」）、処理を終了させる（ステップ１１−４Ｂ）。

ステップ１１−２の条件を設ける理由は、ロボット１００１がユーザによる停止操作やセンサ信号入力待ち等で停止している場合には、粘性摩擦係数Ｄが高いことに起因する衝突トルク推定値τｄｉｓｏの誤差が発生しないからである。

また、図１１に示す動作では、モータ６６の角速度ωｍを監視するが、ロボット１００１が動作していることを確認できる他のパラメータを角速度ωｍの代わりに監視してもよい。例えばロボット１００１のプログラム内で動作命令が実行されている時、位置指令が生成されている時等でロボット動作を判定しても良い。

ピークホールドブロック７４は、トルク推定ブロック３０が動力学トルク推定値τｄｙｎｏを用いて求めた衝突トルク推定値τｄｉｓｏの絶対値がピークホールド値τｄＰより大きいか否かを判定する（ステップ１１−３）。ステップ１１−３にて衝突トルク推定値τｄｉｓｏの絶対値がピークホールド値τｄＰより大きければ（ステップ１１−３の「Ｙｅｓ」）、ピークホールドブロック７４は、ピークホールド値τｄＰを衝突トルク推定値τｄｉｓｏの絶対値にするように更新する。ステップ１１−３にて衝突トルク推定値τｄｉｓｏの絶対値がピークホールド値τｄＰより大きくなければ（ステップ１１−３の「Ｎｏ」）、ピークホールドブロック７４はピークホールド値τｄＰをそのままにする。

ピークホールドブロック７４は、ピークホールドの回数ｉが終了回数ｎｕｍに到達して終了回数ｎｕｍ以上であるか否かを判定する（ステップ１１−４）。ステップ１１−４においてピークホールドの回数ｉが終了回数ｎｕｍ以上であれは（ステップ１１−４の「Ｙｅｓ」）、ピークホールドブロック７４は、ピークホールド値τｄＰを出力し、回数ｉをクリアして０にして（ステップ１１−４Ａ）、ステップ１１−４Ｂにおいて処理を終了する。ステップ１１−４においてピークホールドの回数ｉが終了回数ｎｕｍ以上でなければ（ステップ１１−４の「Ｎｏ」）、ピークホールドブロック７４は、回数ｉをそのままにしてステップ１１−４Ｂにおいて処理を終える。

次に、ピークホールドブロック７４から出力された単位時間でのピークホールド値τｄＰをデータ蓄積ブロック７５で蓄積する。衝突トルク推定値τｄｉｓｏとしてのｘ回目の単位時間（例えば、単位時間が１分間であればｘ分目）のピークホールド値τｄＰをピークホールド値τｄＰ［ｘ］と定義する。現在がｘ回目の単位時間でＮ個のピークホールド値τｄＰを蓄積している場合には、データ蓄積ブロック７５はピークホールド値τｄＰ［ｘ−Ｎ＋１］、τｄＰ［ｘ−Ｎ＋２］、…、τｄＰ［ｘ］を蓄積する。

例えば、衝突検出閾値τｔｈの切換の時点としての現在が３０回目の単位時間であり（ｘ＝３０）、切換の時点より所定時間前（ｘ回だけ前の単位時間で、例えば単位時間が１分であればｘ分前）としてのデータ蓄積個数Ｎが１０個（Ｎ＝１０）である場合には、データ蓄積ブロック７５はピークホールド値τｄＰ［３０］、τｄＰ［２９］、…、τｄＰ［２１］を蓄積していることになる。単位時間が１分間であると、ロボット１００１が動作している１０分間のデータを蓄積することになる。

図１０の最大値抽出ブロック７６は、ピークホールド値τｄＰ［ｘ−Ｎ＋１］、τｄＰ［ｘ−Ｎ＋２］、…、τｄＰ［ｘ］のうちの最大値τｄＰＰを得る。

図１２はエンコーダ５２に内蔵された温度センサ５６の温度センサ出力値Ｔｃと、ｘ回目のピークホールド値τｄＰ［ｘ］を示す。図１２において、縦軸は温度センサ出力値Ｔｃと衝突トルクであるピークホールド値τｄＰ［ｘ］とを示し、横軸は単位時間の回数ｘを示す。図１２において、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。図１２に示す０≦ｘ≦３０の時点ｘでは、実際に衝突は発生しておらず、温度の上昇と共に衝突トルク推定値τｄＰ［ｘ］が減少している場合、つまり粘性摩擦係数Ｄの減少による衝突トルク推定値τｄＰ［ｘ］の誤差が減少している場合を示している。

図１２において、Ｔｃ≧Ｔｃｔｈの条件がｘ＝３０の時点で成立しており、このときの最大値τｄＰＰを示している。データ蓄積個数Ｎは１０個である。

図１０の傾き演算ブロック７７は、衝突トルク推定値τｄＰ［ｘ−Ｎ＋１］〜τｄＰ［ｘ］までの近似直線８３の傾きτｄＰＡを演算する。

図１２において、近似直線８３はｘ＝３０の時点より所定時間前までの衝突トルクの推定値としてのピークホールド値τｄＰ［ｘ］を近似している。近似直線８３の傾きを演算して傾きτｄＰＡを得る。

図１３は図１０の切換え許可判定ブロック７８が実行する処理を示すフローチャートである。次に切換え許可判定ブロック７８は図１３のフローチャートに示す処理を実行する。

切換え許可判定ブロック７８は、まず、切換え許可値ＣＥを「１」以外の数例えば「０」に設定する（クリアする）（ステップ１３−０）。

その後、切換え許可判定ブロック７８は、衝突検出閾値の切換の時点より所定時間前から衝突検出閾値の切換の時点までの期間Ｄｕでの衝突トルクの最大値である最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さいか否かを判定する（ステップ１３−１）。ステップ１３−１において最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さければ（ステップ１３−１の「Ｙｅｓ」）、切換え許可判定ブロック７８は切換え許可値ＣＥを「１」に設定して（ステップ１３−１Ａ）処理を終える。

ステップ１３−１において最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さくない場合（ステップ１３−１の「Ｎｏ」）、切換え許可判定ブロック７８は、最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢより小さいか否かを判定する（ステップ１３−２）。ステップ１３−２にて最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢより小さくない場合には（ステップ１３−２の「Ｎｏ」）、処理を終える。ただし、最大値判定閾値τｄＰＰｔｈＡと最大値判定閾値τｄＰＰｔｈＢと常温時の衝突検出閾値τｔｈとはτｄＰＰｔｈＡ＜τｄＰＰｔｈＢ＜τｔｈの関係を満たすように設定する。

ステップ１３−２にて最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢより小さい場合には（ステップ１３−２の「Ｙｅｓ」）、切換え許可判定ブロック７８は、近似直線８３の傾きτｄＰＡが傾き判定閾値τｄＰＡｔｈより小さいか否かを判定する（ステップ１３−３）。ステップ１３−３において傾き判定閾値τｄＰＡｔｈより小さければ（ステップ１３−３の「Ｙｅｓ」）、切換え許可判定ブロック７８は切換え許可値ＣＥを「１」に設定して処理を終える。ステップ１３−３において傾き判定閾値τｄＰＡｔｈより小さくない場合には（ステップ１３−３の「Ｎｏ」）、切換え許可判定ブロック７８は何もせず処理を終える。

図１２の例では、最大値判定閾値τｄＰＰｔｈＡ、τｄＰＰｔｈＢと傾き判定閾値τｄＰＡｔｈは、τｄＰＰｔｈＡ＝２０、τｄＰＰｔｈＢ＝３５、τｄＰＡｔｈ＝０と設定されている。ｘ＝３０の時点で、τｄＰＰ＜τｄＰＰｔｈＡが成立しているので、切換え許可判定ブロック７８はステップ１３−１で切換え許可値ＣＥを「１」に設定する。

図９において、閾値切換ブロック４１は常温に対応する衝突検出閾値τｔｈと低温に対応する閾値増分ｄτｔｈとを用いて（数７）に従って衝突検出閾値τｖｔｈを衝突判定ブロックに出力する。

（数７）に示すように、閾値切換ブロック４１は、Ｔｃ≧ＴｃｔｈかつＣＥ＝１である場合には、衝突検出閾値τｖｔｈとして常温に対応する衝突検出閾値τｔｈを出力する。また、閾値切換ブロック４１は、Ｔｃ＜Ｔｃｔｈである場合には、許可値ＣＥの値に関わらず、衝突検出閾値τｖｔｈとして常温に対応する衝突検出閾値τｔｈと閾値増分ｄτｔｈとの和を出力する。

なお、温度センサ出力値Ｔｃはモータ６６のエンコーダ５１の内部温度であり減速機５３の温度ではないので、温度閾値Ｔｃｔｈは衝突検出閾値τｔｈと閾値増分ｄτｔｈを定めた際の減速機５３の温度（例えば約５℃）にする必要はなく、マージンを考えて例えば１０℃に設定してもよい。

図１４は閾値切換ブロック４１が実行する（数７）の処理を示すフローチャートである。

閾値切換ブロック４１は、温度センサ出力値Ｔｃが所定の温度閾値Ｔｃｔｈ以上であるか否かを判定する（ステップ１４−１）。ステップ１４−１において温度センサ出力値Ｔｃが所定の温度閾値Ｔｃｔｈ以上でない場合には（ステップ１４−１の「Ｎｏ」）、衝突検出閾値τｖｔｈとして衝突検出閾値τｔｈと閾値増分ｄτｔｈとを加算して得られる（τｔｈ＋ｄτｔｈ）を出力し、許可値ＣＥに「０」を設定して処理を終える。

ステップ１４−１において温度センサ出力値Ｔｃが所定の温度閾値Ｔｃｔｈ以上である場合には（ステップ１４−１の「Ｙｅｓ」）、閾値切換ブロック４１は切換え許可値ＣＥが「１」であるか否かを判定する（ステップ１４−２）。ステップ１４−２において切換え許可値ＣＥが「１」である場合には（ステップ１４−２の「Ｙｅｓ」）、閾値切換ブロック４１は衝突検出閾値τｖｔｈとして常温に対応する衝突検出閾値τｔｈを設定し（ステップ１４−２）、許可値ＣＥはそのままにして処理を終える。

図１２においては、ｘ＝３０の時点で、図１４に示すＴｃ≧Ｔｃｔｈ（ステップ１４−１の「Ｙｅｓ」）とＣＥ＝１（ステップ１４−２の「Ｙｅｓ」）の両方が初めて満たされる。したがって、閾値切換ブロック４１は、ｘ＝３０の時点で衝突検出閾値τｖｔｈは低温に対応する値（τｔｈ＋ｄτｔｈ）から常温に対応する値τｔｈに低下させる。図１５は図１２のｘ＝３０の時点以後の温度センサ出力値Ｔｃとピークホールド値τｄＰとを示す。図１５において、縦軸は温度センサ出力値Ｔｃと衝突トルクのｘ回目のピークホールド値τｄＰ［ｘ］を示し、横軸は単位時間の回数ｘを示す。図１５において、図１２と同様に、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。

図１５に示すように、閾値切換ブロック４１は、衝突検出閾値τｖｔｈはｘ＝３０の時点で、低温に対応する値７０％（＝４０＋３０）から常温に対応する値４０％に低下させる。

図１５に示すように、Ｔｃ≧Ｔｃｔｈの条件が成立するｘ＝３０の時点において、衝突トルク推定値τｄｉｓｏ（衝突トルク推定誤差）が温度Ｔｃの上昇と共に小さくなる。これにより、最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さく、衝突検出閾値τｖｔｈの常温に対応する値τｔｈに比べて十分に小さい。このように、図１５の閾値［％］では、衝突検出閾値τｖｔｈを小さくしても衝突の誤検出が発生しにくい。

以上の様に、実施の形態においてロボット１００１の衝突検出方法では、減速機５３を介してモータ６６により駆動されるロボット１００１において、モータ６６が減速機５３に出力するトルクからロボット１００１の逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値τｄｉｓｏとして推定する。衝突トルク推定値τｄｉｓｏが所定の衝突検出閾値τｔｈより大きければ、ロボット１００１が外力を受けたと判断する。モータ６６のエンコーダ５２に内蔵された温度センサ５６の出力値が所定の温度閾値より高ければ、衝突トルク推定値τｄｉｓｏの衝突検出閾値τｔｈを小さい値に切換える。衝突検出閾値τｔｈの切換えの時点より所定時間前（単位時間でｘ回前、例えば単位時間が１分であればｘ分前）からの衝突トルク推定値τｄｉｓｏの最大値τｄＰＰが、衝突トルク推定値τｄｉｓｏの衝突検出閾値τｔｈの切換え後の値より小さな最大値判定閾値τｄＰＰｔｈＡ未満であれば、衝突検出閾値τｔｈの切換えを許可する。

これにより、エンコーダ５２内蔵の温度センサ５６を用いて衝突検出閾値τｔｈを切換える場合でも、衝突誤検出の発生を減じることができる。

次に図１５以外の動作を説明する。

図１６は実施の形態における衝突検出方法の他の動作における温度センサ出力値Ｔｃとピークホールド値τｄＰとを示す。図１６において、縦軸は温度センサ出力値Ｔｃと衝突トルクのｘ回目のピークホールド値τｄＰ［ｘ］を示し、横軸は単位時間の回数ｘを示す。図１６において、図１５と同様に、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。図１６に示す動作では図１５に比べ、温度センサ出力値Ｔｃの上昇より減速機５３の温度の上昇が低く、粘性摩擦係数Ｄが十分に低下していない。

図１６において、Ｔｃ≧Ｔｃｔｈの条件が成立するｘ＝３０の時点において、最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡ以上、最大値判定閾値τｄＰＰｔｈＢ（７２）未満である。この場合には、図１３のステップ１３−１の「Ｎｏ」とステップ１３−２の「Ｙｅｓ」の条件が成立し、ステップ１３−３の処理が実行される。

図１６に示す動作では図１５に比べ、減速機５３の温度の上昇が低く粘性摩擦係数Ｄが十分に低下していないため、衝突検出閾値τｔｈを小さくした場合、衝突誤検出の発生の可能性が高くなる。そこで図１３のステップ１３−３において図１６のｘ＝３０の時点の近似直線８３の傾きτｄＰＡが傾き判定閾値τｄＰＡｔｈより小さい場合には、切換え許可判定ブロック７８はステップ１３−３Ａにおいて切換え許可値ＣＥを「１」に設定する。ステップ１３−３において図１６のｘ＝３０の時点の近似直線８３の傾きτｄＰＡが傾き判定閾値τｄＰＡｔｈより小さくない場合には切換え許可値ＣＥをそのままにして処理を終了する。

図１６では、傾き判定閾値τｄＰＡｔｈを０に設定している。図１６に示す近似直線８３の傾きτｄＰＡは傾き判定閾値τｄＰＡｔｈすなわち０より小さいので（図１３のステップ１３−３の「Ｙｅｓ」）、衝突トルクが実際には減少傾向にあり衝突誤検出の可能性が低いと判断し、切換え許可判定ブロック７８は切換え許可値ＣＥを「１」に設定し、衝突検出閾値τｖｔｈはｘ＝３０の時点で低温に対応する値７０％（＝４０＋３０）から常温に対応する値４０％に低下させる。

図１７は実施の形態における衝突検出方法のさらに他の動作における温度センサ出力値Ｔｃとピークホールド値τｄＰとを示す。図１７において、縦軸は温度センサ出力値Ｔｃと衝突トルクのｘ回目のピークホールド値τｄＰ［ｘ］を示し、横軸は単位時間の回数ｘを示す。図１７において、図１５や図１６と同様に、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。図１７は、図１６と比較し、ｘ＝３０の時点の近似直線８３の傾きτｄＰＡが傾き判定閾値τｄＰＡｔｈより大きい場合（τｄＰＡ＞０）の動作を示している。

図１７に示す動作では、モータ６６の発熱によりエンコーダ５２内の温度センサ５６の温度センサ出力値Ｔｃが上昇しているにも関わらず、外気温が低く減速機５３の粘性摩擦係数Ｄが増加する場合に相当する。

図１６と図１７では、傾き判定閾値τｄＰＡｔｈは０に設定している。

この場合、衝突トルク推定値τｄｉｓｏ（衝突トルク推定誤差）は増加する傾向、すなわちピークホールド値τｄＰ［ｘ］が増加する傾向にあるので、衝突誤検出を防ぐため、図１３のステップ１３−３で「Ｎｏ」の判定となり、切換え許可値ＣＥは「１」に設定されず、衝突検出閾値τｖｔｈは低温に対応する値（７０％）から低下させない。

図１８は図１７に示す動作でのｘ＝４０の時点での温度センサ出力値Ｔｃと衝突トルクのピークホールド値τｄＰとを更に示す。ｘ＝３０の時点以後も衝突トルク推定値τｄｉｓｏ（衝突トルク推定誤差）は増加する傾向すなわちピークホールド値τｄＰ［ｘ］が増加する傾向にあり、ｘ＝３１、３２、‥、４０で図１０に示す波形分析ブロック５７での処理が行われても、切換え許可値ＣＥが「１」に設定されない。したがって、ｘ＝４０の時点でも衝突検出閾値τｖｔｈは低温に対応する値（７０％）から低下させない。

図１９は実施の形態における衝突検出方法のさらに他の動作における温度センサ出力値Ｔｃとピークホールド値τｄＰとを示す。図１９において、縦軸は温度センサ出力値Ｔｃと衝突トルクのｘ回目のピークホールド値τｄＰ［ｘ］を示し、横軸は単位時間の回数ｘを示す。図１９において、図１５から図１８と同様に、縦軸の衝突トルクや閾値は、最大トルクに対する比である最大トルク比（％）で示している。図１９に示す動作では、図１６に比べ、温度センサ５６の温度センサ出力値Ｔｃより減速機５３の温度上昇がさらに低く、粘性摩擦係数Ｄが低下していない場合の動作を示す。

図１９においてＴｃ≧Ｔｃｔｈの条件が成立するｘ＝３０の時点で、最大値τｄＰＰは最大値判定閾値τｄＰＰｔｈＢより大きい。すなわち、図１３のステップ１３−１での「Ｎｏ」となる条件と、ステップ１３−２での「Ｎｏ」となる条件が成立している。

図２０は図１９に示す動作でのｘ＝３０の時点以後の温度センサ出力値Ｔｃとピークホールド値τｄＰとを更に示す。

図２０において、衝突トルク推定値τｄｉｓｏ（衝突トルク推定誤差）は減少する傾向すなわちピークホールド値τｄＰ［ｘ］が減少する傾向にあり、ｘ＝３５の時点で、図１３のステップ１３−１での「Ｎｏ」の条件とステップ１３−２での「Ｙｅｓ」の条件とステップ１３−３での「Ｙｅｓ」の条件が成立する。したがって、閾値切換ブロック４１は、衝突検出閾値τｖｔｈを低温に対応する値７０％（＝４０＋３０）から常温に対応する値４０％に低下させる。

この様に、実施の形態におけるロボット１００１の衝突検出方法では、最大値判定閾値τｄＰＰｔｈＡより大きく、衝突検出閾値τｖｔｈの切換え後の値より小さな最大値判定閾値τｄＰＰｔｈＢが設けられる。衝突トルク推定値τｄｉｓｏの衝突検出閾値τｖｔｈの切換えの時点より所定時間前からの衝突トルク推定値τｄｉｓｏの最大値τｄＰＰが、最大値判定閾値τｄＰＰｔｈＡ以上で、最大値判定閾値τｄＰＰｔｈＢ未満であり、かつ、衝突検出閾値τｖｔｈの切換えの時点より所定時間前（単位時間でｘ回前、例えば単位時間が１分であればｘ分前）からの衝突トルク推定値τｄｉｓｏの時間に対する推移（ｘ回目のピークホールド値τｄＰ［ｘ］）を近似した近似直線の傾きτｄＰＡが負であれば、衝突トルク推定値τｄｉｓｏの衝突検出閾値τｖｔｈの切換えを許可する。

これにより、エンコーダ５２内蔵の温度センサ５６の温度センサ出力値Ｔｃより減速機５３の温度上昇が低く、粘性摩擦係数Ｄが十分に低下していない場合であっても、衝突の誤検出の発生を減じることができる。

上述のように、ロボット１００１は、モータ６６と、モータ６６に接続された減速機５３と、モータ６６の回転を検出するエンコーダ５１と、エンコーダ５１に内蔵された温度センサ５６と、減速機５３を介してモータ６６により駆動される対象物（アーム６７）とを有する。モータ６６が減速機５３に出力するトルクから、ロボット１００１の逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値τｄｉｓｏ（τｄＰ）として推定する。衝突トルク推定値τｄｉｓｏ（τｄＰ）が所定の衝突検出閾値τｖｔｈより大きければ、ロボット１００１が外力を受けたと判断する。温度センサ５６で検出された温度（Ｔｃ）が所定の温度閾値Ｔｃｔｈ未満である場合に、所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にする。検出された温度（Ｔｃ）が所定の温度閾値Ｔｃｔｈ以上である時点（ｘ＝３０）より所定時間だけ前の時点（ｘ＝２０）から時点（ｘ＝３０）までの期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡより小さい場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）より小さい値τｔｈにする。期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡ以上である場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にする。

最大値判定閾値τｄＰＰｔｈＡより大きくかつ値τｔｈより小さな最大値判定閾値τｄＰＰｔｈＢを設定してもよい。期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の時間推移を近似して得られる近似直線８３の傾きτｄＰＡを得てもよい。この場合に、期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡ以上でかつ最大値判定閾値τｄＰＰｔｈＢ未満であり、近似直線８３の傾きτｄＰＡが負である場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値τｔｈにする。期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＡ以上でかつ最大値判定閾値τｄＰＰｔｈＢ未満であり、近似直線８３の傾きτｄＰＡが０または正である場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にする。期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢ以上である場合に、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にする。

期間Ｄｕでの衝突トルク推定値τｄｉｓｏ（τｄＰ）の最大値τｄＰＰが最大値判定閾値τｄＰＰｔｈＢ以上である場合に、近似直線８３の傾きτｄＰＡに関わらず、時点（ｘ＝３０）において所定の衝突検出閾値τｖｔｈを値（τｔｈ＋ｄτｔｈ）にしてもよい。

以上のように、実施の形態におけるロボット１００１の衝突検出方法により、エンコーダ５２内蔵の温度センサ５６を用いて衝突検出閾値τｖｔｈを切換える場合でも、衝突の誤検出の発生を減じることができる。

本発明のロボットの衝突検出方法は、温度センサによる衝突検出閾値切換えにおいて、コスト上昇や信頼性の低下を招く減速機への温度センサ取り付けをせず、モータエンコーダの内蔵された温度センサを用いた場合でも、衝突誤検出の発生を減じることができ、産業上有用である。

６ 位置制御ブロック
１０ 速度制御ブロック
１７ 伝達関数ブロック
１８ モータ及び外力を示すブロック
２１ 回転方向判定ブロック
２６ トルク演算ブロック
３０ トルク推定ブロック
３１，４０ 衝突判定ブロック
３２ 微分要素
３４，４１ 閾値切換ブロック
５１ エンコーダ
５２ エンコーダ
５３ 減速機
５６ 温度センサ
５７ 波形分析ブロック
６１，１６１ ロボット機構
６２，１６２ ロボット制御装置
６３ 操作・教示部
６４ メイン制御部
６５ サーボ制御部
６６ モータ
６７ アーム
６８ ベルト
７４ ピークホールドブロック
７５ データ蓄積ブロック
７６ 最大値抽出ブロック
７７ 傾き演算ブロック
７８ 切換え許可判定ブロック
８３ 近似直線
６７Ａ（６７１Ａ〜６７６Ａ） 関節軸（対象物）
１００１ ロボット
ＣＥ 切換え許可値
ｄτｔｈ 衝突検出閾値増分
Ｉｍ モータ電流
ＫＩ／ｓ 速度積分ゲイン
ＫＰ 速度比例ゲイン
ＫＰＰ 位置比例ゲイン
Ｋｔ モータトルク定数
Ｋμ 動摩擦係数
ｓｇｎ モータ方向信号
Ｔｃ 温度センサ出力値
Ｔｃｔｈ 温度閾値
αｃｏｍ 加速度成分
θｃｏｍ 位置指令
θＬ 負荷位置
θｍ 位置フィードバック
τｄｉｓ 衝突トルク
τｄｉｓｏ 衝突トルク推定値
τｄＰ ピークホールド値（衝突トルク推定値）
τｄＰ［ｘ］ ピークホールド値
τｄＰＡ 傾き
τｄＰＡｔｈ 傾き判定閾値
τｄＰＰ 最大値
τｄＰＰｔｈＡ 最大値判定閾値（第１の最大値判定閾値）
τｄＰＰｔｈＢ 最大値判定閾値（第２の最大値判定閾値）
τｄｙｎ 動力学トルク
τｄｙｎｏ 動力学トルク推定値
τｍ モータ発生トルク
τｔｈ 衝突検出閾値
τｖｔｈ 衝突検出閾値
τμ 動摩擦トルク
τμｏ 動摩擦トルク推定値
ωｃｏｍ 速度ループ指令
ωｍ モータ角速度

Claims (3)

1. モータと、前記モータに接続された減速機と、前記モータの回転を検出するエンコーダと、前記エンコーダに内蔵された温度センサと、前記減速機を介して前記モータにより駆動される対象物とを有するロボットを準備するステップと、
   前記モータが前記減速機に出力するトルクから、前記ロボットの逆動力学演算で求めた動力学トルクを差し引くことにより、衝突による外力トルクを衝突トルク推定値として推定するステップと、
   前記衝突トルク推定値が所定の衝突検出閾値より大きければ、前記ロボットが外力を受けたと判断するステップと、
   前記温度センサで検出された温度が所定の温度閾値未満である場合に、前記所定の衝突検出閾値を第１の値にするステップと、
   前記検出された温度が前記所定の温度閾値以上である第１の時点より所定時間だけ前の第２の時点から前記第１の時点までの期間での前記衝突トルク推定値の最大値が第１の最大値判定閾値より小さい場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値より小さい第２の値にするステップと、
   前記期間での前記衝突トルク推定値の前記最大値が前記第１の最大値判定閾値以上である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にするステップと、
   を含む、ロボットの衝突検出方法。
2. 前記第１の最大値判定閾値より大きくかつ前記第２の値より小さな第２の最大値判定閾値を準備するステップと、
   前記期間での前記衝突トルク推定値の時間推移を近似して得られる近似直線の傾きを得るステップと、
   をさらに備え、
   前記期間での前記衝突トルク推定値の前記最大値が前記第１の最大値判定閾値以上である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にする前記ステップは、
   前記期間での前記衝突トルク推定値の前記最大値が前記第１の最大値判定閾値以上でかつ前記第２の最大値判定閾値未満であり、前記近似直線の前記傾きが負である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第２の値にするステップと、
   前記期間での前記衝突トルク推定値の前記最大値が前記第１の最大値判定閾値以上でかつ前記第２の最大値判定閾値未満であり、前記近似直線の前記傾きが０または正である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にするステップと、
   前記期間での前記衝突トルク推定値の前記最大値が前記第２の最大値判定閾値以上である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にするステップと、
   を含む、請求項１に記載のロボットの衝突検出方法。
3. 前記期間での前記衝突トルク推定値の前記最大値が前記第２の最大値判定閾値以上である場合に、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にする前記ステップは、前記期間での前記衝突トルク推定値の前記最大値が前記第２の最大値判定閾値以上である場合に、前記近似直線の前記傾きに関わらず、前記第１の時点において前記所定の衝突検出閾値を前記第１の値にするステップを含む、請求項２に記載のロボットの衝突検出方法。

Images