JP7826047B2 - 衝突検出方法及び衝突検出装置 - Google Patents

Description

本発明は、サーボモータによって駆動されるロボットアームやロボットハンドなどの動作対象物における衝突を検出する方法及び装置に関する。

産業用ロボットにおいては軸ごとにモータが設けられており、各軸のモータはその軸に対する位置指令に応じてサーボ制御され、その結果、ロボットのアームやハンドが所望の位置に移動する。この移動の過程においてアームやハンドが周囲の壁面やその他の障害物に衝突することがあり、安全の確保などのために衝突を高精度かつ速やかに検出することが求まれている。衝突センサをロボットのアームやハンドに取り付けて衝突を検出することもできるが、センサを設けることによるコストの上昇やセンサの質量がロボットの運動特性に与える影響などを無視できないので、衝突センサを設けることなく衝突を検出する技術が提案されている。例えば特許文献１，２は、逆動力学演算を行うことによってモータのトルクを推定し、サーボ制御において実際にモータに出力されるトルク指令値と比較し、両者の差が閾値を超えたときに衝突が発生したと検出することを開示している。特許文献３は、指令から計算された速度と実際の速度との差分値、指令から計算された加速度と実際の加速度との差分値、及び加速度の微分値のいずれかを選択し、選択された値が閾値以上となったときに衝突が発生したと判断することを開示している。

特許文献４は、制御系の遅れ時間に基づいて理論上の位置偏差を実時間で算出し、実際の位置偏差との差または比が許容範囲内にないときに衝突が発生したと検知することを開示している。特許文献５は、サーボモータに出力されるトルク指令値を所定周期で検出し、前周期で検出されたトルク指令値と今周期で検出されたトルク指令値との差が閾値以上となったときに衝突が発生したと判断することを開示している。特許文献６は、サーボモータによって駆動されるアームとアームのバランサとして機能してアームに対してトルクを作用させるガススプリングとを備えるロボットにおいて、サーボモータへのトルク指令値と実際にアームを動作させるために必要となったトルクとの差を推定外乱値とし、推定外乱値が閾値を超えたときに衝突が発生したと判断することを開示している。ここではロボットのアームやハンドの衝突の検出についての先行技術を説明したが、ロボット以外の装置においても、サーボモータによって駆動される動作対象物が存在すれば、その動作対象物における衝突を高精度かつ速やかに検出することが必要であり、上述した各先行技術はそのような動作対象物における衝突の検出にも適用することが可能である。

特開２００６－１１６６５０号公報 特開２０１３－１６９６０９号公報 特開２０１６－１９４７６１号公報 特開平８－２２９８６４号公報 特開平１－２３０１０７号公報 特開２０１９－９８４１３号公報

特許文献１，２に開示された技術では、逆動力学演算を行うため、ロボットの機械系における慣性モーメントや粘性摩擦などのパラメータをあらかじめ設定しておく必要があり、ワークの有無や経時変化などによってパラメータが実際のロボットと整合しなくなったときに検出の精度が低下するという課題がある。特許文献３に開示された技術は、モータに付属するエンコーダ出力から算出される速度値あるいは加速度値のみに基づいているため、緩衝構造を有するハンドなどの場合には衝突時の速度変化や加速度変化が小さいので、検出精度が悪くなる、という課題がある。特許文献４に記載の技術も、緩衝構造を有するハンドなどの場合、衝突直後の実際の位置偏差と理論上の位置偏差との差が小さいので、衝突の検出が遅れて検出精度が悪くなる、という課題がある。同様に特許文献５に記載の技術でも、緩衝構造を有するハンドなどの場合、衝突直後のトルク指令値の変動が小さく、衝突の検出が遅れて検出精度が悪くなる、という課題がある。特許文献６に開示された技術は、アームのバランサとして機能するガススプリングを備えることが前提とするものであり、ガススプリングを備えないロボットには適用することができない。

本発明の目的は、サーボモータによって駆動される動作対象物における衝突を、機械系でのパラメータなどに依存することなく、高精度かつ速やかに検出できる衝突検出方法及び装置を提供することにある。

本発明の第１の態様において衝突検出方法は、サーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出方法であって、サーボモータに対するトルク指令値に対してバンドパスフィルタ処理を行い、バンドパスフィルタ処理によって得られる信号の振幅に基づく値と閾値とを比較して、動作対象物における衝突の発生の有無を判定する。このような衝突検出方法によれば、動作対象物において衝突が発生したときにトルク指令値に現れる特定の周波数帯域での変動に基づいて衝突の判定を行うので、衝突の発生を速やかにかつ確実に検出することができる。

本発明の第２の態様において衝突検出方法は、サーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出方法であって、サーボモータに対するトルク指令値に対してバンドパスフィルタ処理を行い、バンドパスフィルタ処理によって得られる信号に対し、動作対象物を含む機械系の固有振動数の成分を減衰させるノッチフィルタ処理を行い、ノッチフィルタ処理を行ったのちの信号の振幅に基づく値と閾値とを比較して、動作対象物における衝突の発生の有無を判定する。このような衝突検出方法によれば、動作対象物において衝突が発生したときにトルク指令値に現れる特定の周波数帯域での変動に基づいて衝突の判定を行うので、衝突の発生を速やかにかつ確実に検出することができる。さらに、ノッチフィルタ処理を行うので、減速機の角度伝達誤差などに起因する振動に対して機械系が共振するような場合において、共振のために衝突検出における誤検出が発生することを抑制することができる。

第１及び第２の態様の衝突検出方法では、バンドパスフィルタ処理は、２次のバタワースフィルタ、３次のバタワースフィルタ及び３次のベッセルフィルタのいずれかによるバンドパスフィルタ処理であることが好ましい。このようなバンドパスフィルタを使用するバンドパスフィルタ処理を行うことによって、衝突の発生を少ない計算量で、より速やかに、かつ、より確実に検出することができる。

本発明の第３の態様において衝突検出方法は、位置指令に基づきサーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出方法であって、位置指令とサーボモータの位置とから算出される位置偏差に対してバンドパスフィルタ処理を行い、バンドパスフィルタ処理によって得られる信号の振幅に基づく値と閾値とを比較して、動作対象物における衝突の発生の有無を判定する。このような衝突検出方法によれば、動作対象物において衝突が発生したときに位置偏差に現れる特定の周波数帯域での変動に基づいて衝突の判定を行うので、衝突の発生を速やかにかつ確実に検出することができる。

本発明の第４の態様において衝突検出方法は、位置指令に基づきサーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出方法であって、位置指令とサーボモータの位置とから算出される位置偏差に対してバンドパスフィルタ処理を行い、バンドパスフィルタ処理によって得られる信号に対し、動作対象物を含む機械系の固有振動数の成分を減衰させるノッチフィルタ処理を行い、ノッチフィルタ処理を行ったのちの信号の振幅に基づく値と閾値とを比較して、動作対象物における衝突の発生の有無を判定する。このような衝突検出方法によれば、動作対象物において衝突が発生したときに位置偏差に現れる特定の周波数帯域での変動に基づいて衝突の判定を行うので、衝突の発生を速やかにかつ確実に検出することができる。さらに、ノッチフィルタ処理を行うので、減速機の角度伝達誤差などに起因する振動に対して機械系が共振するような場合において、共振のために衝突検出における誤検出が発生することを抑制することができる。

第３及び第４の態様の衝突検出方法では、バンドパスフィルタ処理は、３次のバタワースフィルタ、５次のバタワースフィルタ及び３次のベッセルフィルタのいずれかによるバンドパスフィルタ処理であることが好ましい。このようなバンドパスフィルタを使用するバンドパスフィルタ処理を行うことによって、衝突の発生を少ない計算量で、より速やかに、かつ、より確実に検出することができる。

上記の衝突検出方法では、動作対象物を含む機械系の共振が発生する速度領域において他の速度領域に比べて閾値が大きくなるように、サーボモータの速度に応じて閾値を変化させることができる。減速機の角度伝達誤差などにより、サーボモータの速度に比例した振動数の振動が発生し、この振動に対して機械系に共振することがある。サーボモータの速度に応じて閾値を変化させることにより、機械系の共振のために衝突検出における誤検出が発生することを抑制することができる。

上記の衝突検出方法において振幅に基づく値は、例えば、バンドパスフィルタ処理によって得られる信号の絶対値、二乗平均値及び二乗平均平方根値のいずれかである。これらの値を振幅に基づく値とすることによって、衝突の発生を少ない計算量で、より速やかに、かつ、より確実に検出することができる。

上記の衝突検出方法において動作対象物は、例えばロボットのアームまたはハンドである。本発明に基づく衝突検出方法によれば、ロボットの動作に伴ってロボットのアームやハンドが周囲の物体と衝突するときに、その衝突を速やかに、かつ確実に検出することが可能になる。

本発明の第５の態様において衝突検出装置は、サーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出装置であって、サーボモータに対するトルク指令値に対してバンドパスフィルタ処理を行うバンドパスフィルタと、バンドパスフィルタから出力される信号の振幅に基づく値と閾値とを比較して動作対象物における衝突の発生の有無を判定する判定手段と、を有する。このような衝突検出装置によれば、動作対象物において衝突が発生したときにトルク指令値に現れる特定の周波数帯域での変動に基づいて衝突の判定を行うので、衝突の発生を速やかにかつ確実に検出することができる。

本発明の第６の態様において衝突検出装置は、サーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出装置であって、サーボモータに対するトルク指令値に対してバンドパスフィルタ処理を行うバンドパスフィルタと、バンドパスフィルタから出力される信号が供給されて、動作対象物を含む機械系の固有振動数の成分を減衰させるノッチフィルタと、ノッチフィルタから出力される信号の振幅に基づく値と閾値とを比較して動作対象物における衝突の発生の有無を判定する判定手段と、を有する。このような衝突検出装置によれば、動作対象物において衝突が発生したときにトルク指令値に現れる特定の周波数帯域での変動に基づいて衝突の判定を行うので、衝突の発生を速やかにかつ確実に検出することができる。さらに、ノッチフィルタを設けているので、減速機の角度伝達誤差などに起因する振動に対して機械系が共振するような場合において、共振のために衝突検出における誤検出が発生することを抑制することができる。

第５及び第６の態様の衝突検出装置では、バンドパスフィルタは、２次のバタワースフィルタ、３次のバタワースフィルタ及び３次のベッセルフィルタのいずれかからなることが好ましい。このようなバンドパスフィルタを使用することにより、衝突の発生を少ない計算量で、より速やかに、かつ、より確実に検出することができる。

本発明の第７の態様において衝突検出装置は、位置指令に基づきサーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出装置であって、位置指令とサーボモータの位置とから位置偏差を算出する減算器と、位置偏差に対してバンドパスフィルタ処理を行うバンドパスフィルタと、バンドパスフィルタから出力される信号の振幅に基づく値と閾値とを比較して動作対象物における衝突の発生の有無を判定する判定手段と、を有する。このような衝突検出装置によれば、動作対象物において衝突が発生したときに位置偏差に現れる特定の周波数帯域での変動に基づいて衝突の判定を行うので、衝突の発生を速やかにかつ確実に検出することができる。

本発明の第８の態様において衝突検出装置は、位置指令に基づきサーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出装置であって、位置指令とサーボモータの位置とから位置偏差を算出する減算器と、位置偏差に対してバンドパスフィルタ処理を行うバンドパスフィルタと、バンドパスフィルタから出力される信号が供給されて、動作対象物を含む機械系の固有振動数の成分を減衰させるノッチフィルタと、ノッチフィルタから出力される信号の振幅に基づく値と閾値とを比較して動作対象物における衝突の発生の有無を判定する判定手段と、を有する。このような衝突検出装置によれば、動作対象物において衝突が発生したときに位置偏差に現れる特定の周波数帯域での変動に基づいて衝突の判定を行うので、衝突の発生を速やかにかつ確実に検出することができる。さらに、ノッチフィルタを設けているので、減速機の角度伝達誤差などに起因する振動に対して機械系が共振するような場合において、共振のために衝突検出における誤検出が発生することを抑制することができる。

第７及び第８の態様の衝突検出装置では、バンドパスフィルタは、３次のバタワースフィルタ、５次のバタワースフィルタ及び３次のベッセルフィルタのいずれかからなることが好ましい。このようなバンドパスフィルタを使用することにより、衝突の発生を少ない計算量で、より速やかに、かつ、より確実に検出することができる。

上記の衝突検出装置は、動作対象物を含む機械系の共振が発生する速度領域において他の速度領域に比べて閾値が大きくなるように、サーボモータの速度に応じて閾値を変化させる閾値変化手段を備えてもよい。減速機の角度伝達誤差などにより、サーボモータの速度に比例した振動数の振動が発生し、この振動に対して機械系に共振することがあるが、閾値変化手段を設けてサーボモータの速度に応じて閾値を変化させることにより、機械系の共振のために衝突検出における誤検出が発生することを抑制することができる。この場合、閾値変化手段は、サーボモータの速度が入力して前記入力した速度に応じた閾値を出力するルックアップテーブルによって構成することができる。ルックアップテーブルを用いることにより、閾値変化手段を容易に構成することが可能になる。

上記の衝突検出装置において振幅に基づく値は、例えば、バンドパスフィルタから出力される信号の絶対値、二乗平均値及び二乗平均平方根値のいずれかである。これらの値を振幅に基づく値とすることによって、衝突の発生を少ない計算量で、より速やかに、より、より確実に検出することができる。

上記の衝突検出装置は、例えば、外部から与えられる指令値に基づいてサーボモータの制御を実行する制御装置の内部に設けられる。制御装置の内部に衝突検出装置を設けることにより、全体としての装置構成がコンパクトになるとともに、衝突の発生を検出したときにサーボモータを停止させるなどの制御を行なうことが容易になる。

本発明によれば、サーボモータによって駆動される動作対象物における衝突を、機械系でのパラメータなどに依存することなく、高精度かつ速やかに検出できる。

（ａ）は、本発明に基づく衝突検出方法を適用可能なロボットの一例の全体構成を示す概略図であり、（ｂ）はロボットの正面図である。 （ａ），（ｂ）はそれぞれロボットの側面図と平面図である。 （ａ）は、本発明の第１の実施形態の衝突検出装置を備える制御装置の構成を示すブロック図であり、（ｂ）は、ＲＭＳ演算部の構成を示すブロック図である。 衝突検出の例を説明する波形図である。 衝突検出の例を説明する波形図である。 本発明の第２の実施形態の衝突検出装置を備える制御装置の構成を示すブロック図である。 衝突検出の例を説明する波形図である。 （ａ）は、本発明の第３の実施形態の衝突検出装置を備える制御装置の構成を示すブロック図であり、（ｂ）は、閾値の設定を説明するグラフである。 第３の実施形態の衝突検出装置の別の例を示すブロック図である。 第３の実施形態の衝突検出装置のさらに別の例を示すブロック図である。 第３の実施形態の衝突検出装置のさらに別の例を示すブロック図である。 第３の実施形態の衝突検出装置のさらに別の例を示すブロック図である。

次に、本発明を実施するための形態について、図面を参照して説明する。本発明は、サーボモータによって駆動される動作対象物における衝突検出に関するものであるが、ここでは、動作対象物が産業用ロボットであるものとして説明を行う。各実施形態の説明に先立ち、衝突検出の対象となる動作対象物の一例であるロボットについて、図１及び図２を用いて説明する。図１において（ａ）はロボット１０の全体構成を示す概略図であり、（ｂ）はロボット１０の正面図である。図２において（ａ）及び（ｂ）は、それぞれ、ロボット１０の側面図及び平面図である。

図示されるロボット１０は、ガラス基板などのワーク５０を搬送することを目的とするものであって、水平多関節ロボットとして構成されている。特にこのロボット１０は、ワーク５０（図１には不図示）をそれぞれ保持する２つのハンド１３Ａ，１３Ｂを備えるいわゆるダブル・ハンド・ロボットとして構成されている。ロボット１０には、ロボット１０に対する動作指令が外部から入力し、この動作指令に基づいてロボット１０を駆動し制御する制御装置（ロボットコントローラ）４０が、ケーブルによって電気的に接続されている。後述するように制御装置４０には、ロボット１０における衝突の発生を検出する衝突検出装置４２（図３参照）が設けられている。なお図１（ａ）は、ロボット１０において水平多関節機構が上昇した状態を示しているのに対し、図１（ｂ）、図２（ａ），（ｂ）は、水平多関節機構が下降した状態を示している。

ロボット１０は、床面ＦＬ（図１（ｂ）参照）において直線状に設けられた相互に平行な１対のレール２１上を移動可能な基台２２と、基台２２の上に設けられて回転軸３１の周りで水平面内で回転する回転台２３と、回転台２３に対して直立するように設けられた昇降機構２４を備えている。基台２２には、レール２１に沿ってロボット１０の全体を水平移動させるためのモータが取り付けられている。また基台２２には、基台２２に対して回転軸３１の周りで回転台２３を回転させるために、水平移動用のモータとは別のθ軸モータとも呼ばれるモータが設けられている。昇降機構２４は、回転台２３に取り付けられている固定部２４Ａと、モータによって固定部２４Ａに対して昇降する移動部２４Ｂとを備えている。移動部２４Ｂを昇降させるモータは、固定部２４Ａに設けられており、移動部２４Ｂは、昇降用のモータによって駆動されて昇降する。

移動部２４Ｂには水平多関節機構を保持するアーム支持部２６Ａ，２６Ｂが水平方向に延びるように設けられており、アーム支持部２６Ａ，２６Ｂの先端にはそれぞれ水平多関節機構が取り付けられている。上側の水平多関節機構は、アーム支持部２６Ａに取り付けられて共通軸３２の周りで水平面内を回転可能な第１アーム１１Ａと、第１アーム１１Ａの先端に取り付けられて軸３３Ａの周りで水平面内を回転可能な第２アーム１２Ａを備えており、第２アーム１２Ａの先端にハンド１３Ａが取り付けられている。同様に下側の水平多関節機構は、アーム支持部２６Ｂに取り付けられて共通軸３２の周りで水平面内を回転可能な第１アーム１１Ｂと、第１アーム１１Ｂの先端に取り付けられて軸３３Ｂの周りで水平面内を回転可能な第２アーム１２Ｂを備えており、第２アーム１２Ｂの先端にハンド１３Ｂが取り付けられている。

ハンド１３Ａ，１３Ｂは、下から保持することによって板状のワーク５０を水平状態に保ったまま搬送できるように、複数の棒状部材を平行に配置したフォーク状の形状となっている。すなわちハンド１３Ａ，１３Ｂは、ワーク５０をそのハンド１３Ａ，１３Ｂの上に保持する形式のものである。ハンド１３Ａ，１３Ｂは、ロードロック室やカセットなどに収納されているワーク５０を取り出してハンド１３Ａ，１３Ｂ上に保持したり、保持しているワーク５０をロードロック室内などに収納するときにワーク５０に対して前進または後退するが、このハンド１３Ａ，１３Ｂの前進したり後退する方向は、棒状部材の延びる方向と平行な方向とされる。

ロボット１０において水平多関節機構は、第１アーム１１Ａ，１１Ｂと第２アーム１２Ａ，１２Ｂとに組み込まれたリンク機構により、アーム支持部２６が延びる方向とは直交する方向で直線運動でハンド１３Ａ，１３Ｂが前進及び後退運動を行うように構成されている。すなわち両方のハンド１３Ａ，１３Ｂは同一方向に前進及び後退を行う。共通軸３２に対してハンド１３Ａ，１３Ｂの先端が遠ざかる動きが前進運動であり、前進運動とは反対方向の動きが後退運動である。第１アーム１１Ａ，１１Ｂと第２アーム１２Ａ，１２Ｂとは全体して屈曲運動を行い、それにも関わらず水平面内でのハンド１３Ａ，１３Ｂの向きを一定とするために、ハンド１３Ａ，１３Ｂは、それぞれ、第２アーム１２Ａ，１２Ｂの先端の位置で手首軸３４Ａ，３４Ｂの周りで水平面内を回転可能に取り付けられている。上側の水平多関節機構では、アーム支持部２６に設けられたモータにより駆動されることによって、第１アーム１１Ａ及び第２アーム１２Ａが動き、ハンド１３Ａはその向きを保ったまま、アーム支持部２６の延びる方向とは直交する方向に移動する。同様に下側の水平多関節機構では、アーム支持部２６に設けられたモータにより駆動されることによって、第１アーム１１Ｂ及び第２アーム１２Ｂが動き、ハンド１３Ｂはその向きを保ったまま、アーム支持部２６の延びる方向とは直交する方向に移動する。ロボット１０では、２つのハンド１３Ａ，１３Ｂを独立して前進及び後退させることができる。以下の説明において、第１アーム１１Ａ，１１Ｂと第２アーム１２Ａ，１２Ｂとに組み込まれたリンク機構によりハンド１３Ａ，１３Ｂが前進及び後退運動をすることをアームの伸縮運動と呼ぶ。

結局、ロボット１０での動きは、レール２１に沿った水平方向の移動（これをＸ軸あるいは走行軸の動きとする）と、回転軸３１の周りでの基台２２に対する昇降機構２４の旋回（これをθ軸あるいは旋回軸の動きとする）と、ハンド１３Ａ，１３Ｂの前進及び後退運動、すなわちアーム伸縮運動（これをＲ軸の動きとする）と、昇降機構２４によるアーム支持部２６の昇降（これをＺ軸の動きとする）とに分けることができ、これらは、それぞれロボット１０に設けられている軸ごとのモータによって駆動される。ロボット１０は、移動動作に対応する指令に応じた制御装置４０からの駆動制御により、これらの各軸のうちの１つの軸だけを動かす動作や、２以上の軸を同時に動かす動作とを実行する。

ロボット１０を用いたガラス基板などのワーク５０の搬送について説明する。ここではレール２１に沿って２つのロードロック室が設けられており、これらのロードロック室の間でワーク５０を搬送する場合を考える。まず、ハンド１３Ａ，１３Ｂがいずれも後退して第１アーム１１Ａ，１１Ｂと第２アーム１２Ａ，１２Ｂとが折り畳まれた状態、すなわちアームが縮んだ状態で、搬出側のロードロック室に向き合う位置にまでレール２１に沿ってロボット１０を移動させ、同時に、ハンド１３Ａの高さをロードロック室に対応する高さとなるように、昇降機構２４によって昇降させる。そして、アームを伸ばすことによりハンド１３Ａを前進させて搬出側のロードロック室内にハンド１３Ａを進入させ、わずかにハンド１３Ａを上昇させることによってハンド１３Ａ上にワーク５０を載置する。その後、アームを縮めることによりハンド１３Ａを後退させてワーク５０ごとハンド１３Ａをロードロック室から引き出す。次に、ワーク５０を搭載した状態で搬入側のロードロック室に向き合う位置にまでレール２１に沿ってロボット１０を移動させ、同時にハンド１３Ａの高さがロードロック室に対応する高さとなるように、昇降機構２４によってアーム支持部２６Ａを昇降させる。そして、ワーク５０を載せたままハンド１３Ａを前進させて第２のカセット内にハンド１３Ａを進入させ、わずかにハンド１３Ａを下降させることによって搬入側のロードロック室にワーク５０を載置し、ハンド１３Ａを後退させてロードロック室からハンド１３Ａを引き出す。以上の動作によって、ワーク５０が搬送されたことになる。レール２１を挟んで一方の側に搬出側のロードロック室が配置し、他方の側に搬入側のロードロック室が配置する場合は、上記の動作に加え、回転軸３１の周りでの旋回の動作が実行される。

上述したロボット１０では、各軸のモータを駆動することによって、例えばハンド１３Ａ，１３Ｂを所望の位置に移動させることができる。この移動の過程によって、ロボット１０の一部、例えばハンド１３Ａ，１３Ｂが、他の物体、例えばロボット１０の周囲の壁面に衝突することがある。以下、ロボット１０における衝突の発生を検出するための機構について説明する。

［第１の実施形態］
図３（ａ）は、ロボット１０に接続する制御装置４０の構成を示すブロック図であり、本発明の第１の実施形態の衝突検出装置４２を説明するものである。制御装置４０は、入力する指令値、例えば位置指令値に基づいてロボット１０の各軸のモータ３６を駆動し制御する。モータ３６には、そのモータ３６の回転位置を検出するエンコーダ３７が取り付けられている。モータ３６の回転軸には減速機３８が取り付けられており、減速機３８を介して負荷３９がモータ３６に機械的に接続する。制御装置４０には、モータ３６の位置を示す信号がエンコーダ３７からフィードバックされてモータ３６をサーボ制御する制御部４１と、ロボット１０における衝突の発生を検出する第１の実施形態の衝突検出装置４２とが設けられている。図では、モータ３６、制御部４１及び衝突検出装置４２が１つずつ描かれているが、ロボット１０には複数のモータ３６が設けられるので、制御装置４０では、その制御するモータ３６ごとに制御部４１と衝突検出装置４２が設けられる。したがって制御装置４０では、モータ３６の駆動及び制御と、衝突の発生の検出とが、軸ごとに実行される。もっとも、ロボット１０において衝突が発生しやすい軸は限られており、また、例えば、回転軸３１での運動（回転台２３での回転運動）の結果、衝突が発生したとしても、それはアーム１１Ａ，１１Ｂ，１２Ａ，１２Ｂあるいはハンド１３Ａ，１３Ｂでの衝突として検出できるから、必ずしもロボット１０のすべての軸において衝突検出を実行する必要はなく、衝突検出を行わない軸については衝突検出装置４２を設ける必要はない。制御部４１としては、産業用ロボットの各軸のモータを制御するための標準的なサーボ制御機構が使用される。

衝突検出装置４２は、制御部４１からモータ３６に与えられるトルク指令値を示す信号に対してバンドパスフィルタ処理を行うバンドパスフィルタ４４と、バンドパスフィルタ処理後のトルク指令値の絶対値を算出する絶対値演算部４５と、絶対値演算部４５において算出された絶対値と閾値とを比較するコンパレータ４６とを備えている。モータ３６による駆動対象物であるロボット１０において衝突が発生したときは、ロボット１０を移動させるためにトルク指令値に発生する変動とは独立して、サーボ制御を行なっていること自体に起因して、トルク指令値において特定の周波数帯域の微小な振動が発生すると考えられる。バンドパスフィルタ４４は、この特定の周波数帯域での微小な振動を抽出するために用いられている。絶対値演算部４５において算出された絶対値が閾値を上回っていれば、衝突の発生を検出したこととなり、コンパレータ４６の出力が衝突検出結果となる。さらに衝突検出装置４２は、コンパレータ４６での演算により衝突の発生を検出したときに警報を出力する警報処理部４７を備えている。

バンドパスフィルタ４４は、例えばローパスフィルタ４４Ｌとハイパスフィルタ４４Ｈとをカスケード接続して構成される。ローパスフィルタ４４Ｌ及びハイパスフィルタ４４Ｈはいずれもバタワースフィルタあるいはベッセルフィルタであることが好ましい。バタワースフィルタを用いる場合には、ローパスフィルタ４４Ｌ及びハイパスフィルタ４４Ｈに同じ次数のバタワースフィルタを使用する。バタワースフィルタの次数は例えば、２次、３次、５次であり、２次あるいは３次であることが好ましい。ベッセルフィルタを用いる場合にも、ローパスフィルタ４４Ｌ及びハイパスフィルタ４４Ｈに同じ次数のベッセルフィルタを使用する。ベッセルフィルタの次数は例えば、３次である。

バンドパスフィルタ４４の通過帯域は、モータ３６の仕様や、ロボット１０自体の大きさや構成などによって最適なものが異なるが、バタワースフィルタを用いる場合であれば、例えば、２～２０Ｈｚとされる。この場合、ローパスフィルタ４４Ｌの遮断周波数を２０Ｈｚとし、ハイパスフィルタ４４Ｈの遮断周波数を２Ｈｚとする。なお、ベッセルフィルタの場合には、ここでの説明におけるローパスフィルタ４４Ｌ及びハイパスフィルタ４４Ｈの遮断周波数の代わりに特性周波数を使用する。例えば２～２０Ｈｚの通過帯域とするときは、ローパスフィルタ４４Ｌに用いるベッセルフィルタの特性周波数を２０Ｈｚとし、ハイパスフィルタ４４Ｈに用いるベッセルフィルタの特性周波数を２Ｈｚとする。

ロボットの制御に用いられる制御装置は、一般に、ソフトウェアによって動作するマイクロプロセッサなどによって実現されるので、本実施形態においても衝突検出装置４２は、例えばマイクロプロセッサなどを用いたデジタル信号処理技術を用いて実現される。その場合、バンドパスフィルタ４４はデジタルフィルタ技術を用いて実装される。

衝突検出装置４２では、絶対値演算部４５の代わりに、バンドパスフィルタ処理後のトルク指令値の二乗平均平方根（ＲＭＳ；Root Mean Square）値を算出するＲＭＳ演算部４８を設け、算出されたＲＭＳ値がコンパレータ４６に入力するようにしてもよい。図３（ｂ）に示すようにＲＭＳ演算部４８は、バンドパスフィルタ４４で処理された速度値を二乗する二乗演算部（Ｓｑ）４８Ａと、二乗演算部４８Ａで算出された値を所定の長さの期間にわたって積算する積算部（Ｓｕｍ）４８Ｂと、積算部４８Ｂで積算された値の平方根を求める平方根演算部（Ｓｑｒｔ）４８Ｃとを備えている。ここではデジタル信号処理を前提としているので、積算部４８Ｂは、信号の各サンプリング周期ごとに、直前の所定の数のサンプリング周期において二乗演算部４８Ａで算出された値を積算し、その値をＲＭＳ値として出力する。なお、所定の数のサンプリング周期を１サンプリング周期とすれば、ＲＭＳ演算部４８での演算処理は絶対値演算部４５での演算処理と同一の結果を与える。

衝突検出装置４２においてトルク指令値に対するバンドパスフィルタ処理を行った後にそのＲＭＳ値を求めて閾値を比較することにより衝突の発生の有無を検出することを説明したが、ＲＭＳ値と閾値との比較による判定だけであれば、ＲＭＳ演算部４８の平方根演算部４８Ｃは必ずしも必要ではなく、積算部４８Ｂの出力と閾値とを比較するような構成とすることもできる。この場合は、バンドパスフィルタ処理後のトルク指令値の二乗平均と閾値とを比較していることになり、このとき用いる閾値は、ＲＭＳ値用のものではなくて二乗平均値用のものとされる。絶対値も二乗平均値もＲＭＳ値も振幅に基づく値であるといえるから、結局、本実施形態の衝突検出装置４２は、モータ３６に対するトルク指令値を示す信号をバンドパスフィルタ４４で処理した後、フィルタ処理後の信号の振幅に基づく値と閾値とを比較し、振幅に基づく値が閾値を上回るときにロボット１０において衝突が発生したと判定するものである。

以下、本実施形態の衝突検出装置４２を用いた衝突検出を実例に基づいて説明する。ここでは図１及び図２に示したロボット１０を所定の位置指令パターンで駆動することとして、ロボット１０の下側のハンド１３Ｂの移動経路内に物体が存在してハンド１３Ｂにおいて衝突が発生したときと、移動経路内に物体が存在せずに衝突が発生しないときとを比較しながら、ハンド１３Ｂを駆動するモータ３６に対するトルク指令値に基づいて衝突の発生を検出した場合を説明する。衝突の検出には、モータ３６を駆動する制御装置４０に組み込まれた、図３（ａ）に示す衝突検出装置４２を使用した。ただし衝突検出装置４２において、絶対値演算部４５の代わりに、積算部４８Ｂでの積算時間０．０１秒であるＲＭＳ演算部４８（図３（ｂ）参照）を使用した。積算部４８Ｂでの積算時間が短いので、ＲＭＳ演算部４８が出力するＲＭＳ値は、バンドパスフィルタ処理後の信号の絶対値とほぼ一致する。結果を図４及び図５に示す。図４及び図５において、縦軸の「トルク」の単位は、制御装置４０の内部において演算に使用される単位（制御装置４０に依存して例えば「ｄｉｇｉｔ」と呼ばれる単位）で表されている。

図４において（ａ－１）は、ハンド１３Ｂにおいて衝突が起きなかったときにモータ３６に与えられたトルク指令値の時間変化を示し、（ｂ－１）は衝突が起きたときのトルク指令値の時間変化を示している。衝突の発生時刻がちょうど横軸の１秒のところであることは分かっている。トルク指令値の波形は異なっているが、ハンド１３Ｂをどのように動かすかの制御によっては衝突が起こらなくても（ｂ－１）に示すようにトルク指令値が変化することがあるから、（ａ－１）の波形と（ｂ－１）の波形を比較しただけでは衝突の発生の有無を判定することはできない。

図４において（ａ－２）及び（ｂ－２）は、それぞれ、波形（ａ－１）及び波形（ｂ－１）で示されるトルク指令値を、通過帯域が２～２０Ｈｚである５次のバタワースフィルタをバンドパスフィルタ４４として使用してフィルタ処理したときの、フィルタ処理後の信号のＲＭＳ値の時間変化を示している。（ａ－２）に示す衝突が発生しなかった場合にはフィルタ処理後の信号から得たＲＭＳ値は、トルク指令値自体での大きな変動にも関わらずほぼ０であってわずかしか変動しない。これに対し、（ｂ－２）に示す衝突が発生した場合には、衝突発生時刻を境としてＲＭＳ値に大きな変動が見られた。（ｂ－２）における時刻１秒におけるＲＭＳ値に基づいて例えばトルク値５００を閾値とすれば、モータ３６に対するトルク指令値に対してバンドパスフィルタ処理を行い、さらに絶対値あるいはＲＭＳ値を求めて閾値を比較することにより、モータ３６によって駆動されるハンド１３Ｂにおける衝突を速やかかつ確実に検出できることが分かる。

同様に図４において、（ａ－３），（ａ－４）及び（ａ－５）は、波形（ａ－１）で示されるトルク指令値を異なるバンドパスフィルタ４４で処理して得た信号のＲＭＳ値を求めたときにＲＭＳ値の時間変化を示し、（ｂ－３），（ｂ－４）及び（ｂ－５）は、波形（ｂ－１）で示されるトルク指令値を異なるバンドパスフィルタ４４で処理して得た信号のＲＭＳ値を求めたときにＲＭＳ値の時間変化を示している。（ａ－３）及び（ｂ－３）では、通過帯域が２～２０Ｈｚである３次のバタワースフィルタを使用し、（ａ－４）及び（ｂ－４）では、通過帯域が２～２０Ｈｚである２次のバタワースフィルタを使用し、（ａ－５）及び（ｂ－５）では、通過帯域が１０～２０Ｈｚである３次のバタワースフィルタを使用している。これらの結果から、通過帯域が２～２０Ｈｚである２次あるいは３じのバタワースフィルタを用いたとき（（ａ－３）、（ｂ－３）、（ａ－４）及び（ｂー４））も、衝突がないときはＲＭＳ値はほぼ０であるのに対し、衝突が発生したとはＲＭＳ値が大きく変動し、良好に衝突を検出できることが分かった。通過帯域は２～２０Ｈｚと同じであるが５次のバタワースフィルタを用いるときより３次のバタワースフィルタを用いたときの方が、衝突発生時におけるＲＭＳ値の立ち上がりが早かった。オフセット（衝突が発生していないときのＲＭＳ値）は、３次のバタワースフィルタを用いる場合に比べ２次のバタワースフィルタを用いるときの方が大きかった。これらに対し、通過帯域を１０～２０Ｈｚとした３次のバタワースフィルタを用いたとき（（ａ－５）及び（ｂ－５））は、衝突が発生したときのＲＭＳ値の変動が小さく、衝突を有無を判別するように図において一点鎖線で示すように閾値を設定したときに、実際の衝突の発生からＲＭＳ値が閾値を上回るまでの遅延が大きくなり、速やかな衝突の検出という点ではやや劣る結果となった。

図５も図４と同様に、異なるバンドパスフィルタ４４を用いた時のＲＭＳ値の時間変化を示している。図５の（ａ－１）及び（ｂ－１）はトルク指令値の時間変化を示す図であって、比較を容易にするために図４の（ａ－１）及び（ｂ－１）を再掲したものである。図５において、（ａ－６）、（ａ－７）及び（ａ－８）は、波形（ａ－１）をバンドパスフィルタ４４で処理して得られたＲＭＳ値の時間変化を示し、（ｂ－６），（ｂ－７）及び（ｂ－８）は、波形（ｂ－１）をバンドパスフィルタ４４で処理して得られるＲＭＳ値の時間変化を示している。（ａ－６）及び（ｂ－６）では、通過帯域が２～２０Ｈｚである１次フィルタを使用し、（ａ－７）及び（ｂ－７）では、通過帯域が０．８～８Ｈｚである３次のベッセルフィルタを使用し、（ａ－８）及び（ｂ－８）では、通過帯域が２～２０Ｈｚである３次のベッセルフィルタを使用している。１次フィルタを用いたとき（（ａ－６）及び（ｂ－６））は、衝突発生時のＲＭＳ値の変化は大きいが、衝突が起きていないときであってもトルク指令値の変化に応じてＲＭＳ値も変動しオフセットが大きいので、衝突の発生を正確に検出するという点ではやや劣る結果となった。一方、ベッセルフィルタを用いたとき（（ａ－７）、（ｂ－７）、（ａ－８）及び（ｂ－８））は、いずれも、衝突が起きていないときのオフセットが十分に小さく、衝突が発生したときにＲＭＳ値が大きく変動するので、ベッセルフィルタによってもロボット１０における衝突の発生を速やかかつ正確に検出できることが分かった。なお、ベッセルフィルタを用いるときは、バタワースフィルタを用いる場合に比べてバンドパスフィルタ４４の通過帯域を低周波側にずらした方が、衝突時のＲＭＳ値の変動が大きくなるので、より好ましい結果を示した。

以上説明した第１の実施形態によれば、モータ３６に対するトルク指令値をバンドパスフィルタ４４で処理し、フィルタ処理後のトルク指令値を示す信号の振幅に基づく値を閾値と比較することにより、ロボット１０において発生した衝突を速やかかつ正確に検出することができる。

［第２の実施形態］
第１の実施形態の衝突検出装置４２は、モータ３６に対するトルク指令値をバンドパスフィルタ処理することによって衝突を検出していたが、本発明に基づく衝突検出装置４２はこれに限られるものではない。モータ３６に関する位置偏差に基づいても、ロボット１０における衝突の検出を行うことができる。図６は、本発明の第２の実施形態に基づく衝突検出装置４２を含む制御装置４０を示している。

図６に示す制御装置４０は、図３（ａ）に示した制御装置４０と同様のものであるが、衝突検出装置４２の構成において図３（ａ）に示すものと異なっている。図６に示す第２の実施形態の衝突検出装置４２には、トルク指令値が入力する代わりに、制御部４１に与えられる位置指令値と、エンコーダ３７からフィードバックされるモータ３６の位置を示す信号とが入力する。衝突検出装置４２は、位置指令値からモータ３６の位置を減算して位置偏差を算出する減算器４３を備えており、減算器４３の出力がバンドパスフィルタ４４に供給される。その他の点では図６に示す衝突検出装置４２は、図３（ａ）に示す衝突検出装置４２と同じ構成である。第２の実施形態においても、絶対値演算部４５の代わりに図３（ｂ）に示すＲＭＳ演算部４８を用いてもよい。本実施形態の衝突検出装置４２では、減算器４３において算出された位置偏差がバンドパスフィルタ４４によってフィルタ処理され、フィルタ処理後の位置偏差の絶対値あるいはＲＭＳ値が算出され、算出された絶対値あるいはＲＭＳ値が閾値を上回るときに、ロボット１０において衝突が発生したと判定される。モータ３６による駆動対象物であるロボット１０において衝突が発生したときは、位置指令値に基づいてロボット１０を移動させるために発生する位置偏差における比較的ゆっくりした変化とは独立して、衝突自体に起因して、位置偏差において特定の周波数帯域の微小な振動が発生すると考えられる。第２の実施形態においてバンドパスフィルタ４４は、位置偏差における特定の周波数帯域での微小な振動を抽出するために用いられている。

以下、図６に示す衝突検出装置４２を用いた衝突検出を実例に基づいて説明する。ここでは図１及び図２に示したロボット１０を所定の位置指令パターンで駆動することとして、ハンド１３Ｂの移動経路内に物体が存在して衝突が発生したときと、移動経路内に物体が存在せずに衝突が発生しないときとを比較しながら、ハンド１３Ｂを駆動するモータ３６での位置偏差に基づいて衝突の発生を検出した場合を説明する。衝突検出装置４２では、絶対値演算部４５の代わりに、積算部４８Ｂでの積算時間０．０１秒であるＲＭＳ演算部４８を使用した。積算部４８Ｂでの積算時間が短いので、ＲＭＳ演算部４８が出力するＲＭＳ値は、バンドパスフィルタ処理後の信号の絶対値とほぼ一致する。結果を図７に示す。図７において、縦軸の「位置偏差」の単位は、制御装置４０の内部において演算に使用される長さの単位（制御装置４０に依存して例えば「ｐｕｌｓｅ」と呼ばれる単位）で表されている。

図７において（ａ－１）は、ハンド１３Ｂにおいて衝突が起きなかったときのモータ３６の位置偏差の時間変化を示し、（ｂ－１）は衝突が起きたときの位置偏差の時間変化を示している。衝突の発生時刻がちょうど横軸の１秒のところであることは分かっている。位置偏差の波形は異なっているが、ハンド１３Ｂをどのように動かすかの制御などによっては衝突が起こらなくても（ｂ－１）に示すように位置偏差が変化することがあるから、（ａ－１）の波形と（ｂ－１）の波形を比較しただけでは衝突の発生の有無を判定することはできない。

図７において（ａ－２）及び（ｂ－２）は、それぞれ、波形（ａ－１）及び波形（ｂ－１）で示される位置偏差を、通過帯域が２～２０Ｈｚである５次のバタワースフィルタをバンドパスフィルタ４４として使用してフィルタ処理したときの、フィルタ処理後の信号のＲＭＳ値の時間変化を示している。（ａ－２）に示す衝突が発生しなかった場合にはフィルタ処理後の信号から得たＲＭＳ値は、位置偏差自体での大きな変動にも関わらずほとんど変化しない。これに対し、（ｂ－２）に示す衝突が発生した場合には、衝突発生時刻を境としてＲＭＳ値に大きな変動が見られた。（ｂ－２）における時刻１秒におけるＲＭＳ値に基づいて例えば位置偏差２０を閾値とすれば、モータ３６での位置偏差に対してバンドパスフィルタ処理を行い、さらに絶対値あるいはＲＭＳ値を求めて閾値を比較することにより、モータ３６によって駆動されるハンド１３Ｂにおける衝突を速やかかつ確実に検出できることが分かる。

同様に図７において（ａ－３）及び（ｂ－３）は、それぞれ、波形（ａ－１）及び波形（ｂ－１）で示される位置偏差を、通過帯域が２～２０Ｈｚである３次のバタワースフィルタをバンドパスフィルタ４４として使用してフィルタ処理したときの、フィルタ処理後の信号のＲＭＳ値の時間変化を示している。３次のバタワースフィルタを使用した場合であっても衝突の検出を行うことができる。しかしながら（ａ－２）及び（ａ－３）に示す５次のバタワースフィルタを使用した場合に比べ、３次のバタワースフィルタを使用したときは、衝突が発生していないときのオフセットが大きく、その分、衝突の正確な検出という点で５次のバタワースフィルタを用いるときよりも若干劣っていた。すなわち、位置偏差に基づいて衝突の検出を行うときは、トルク指令値に基づいて衝突の検出を行うときに比べ、使用するバンドパスフィルタ４４の次数を高めることが好ましいことが分かった。これは、位置偏差の２回微分がトルク指令値に対応するという関係があるので、オフセットの除去のために位置偏差を用いるときは、トルク指令値を用いるときよりもフィルタの次数を高くする必要があるからである。このことから、位置偏差に基づいて衝突の発生を検出するときにバンドパスフィルタ４４としてバタワースフィルタを用いるときは、２次のバタワースフィルタを用いるよりも３次または５次のバタワースフィルタを用いることが好ましいことが分かる。ここでは具体例を示さないが、位置偏差に基づいて衝突の発生を検出するときにベッセルフィルタを用いるときは、ベッセルフィルタの次数は例えば３次のままでよい。

以上説明した第２の実施形態によれば、モータ３６での位置偏差をバンドパスフィルタ４４で処理し、フィルタ処理後の位置偏差を示す信号の振幅に基づく値を閾値と比較することにより、ロボット１０において発生した衝突を速やかかつ正確に検出することができる。

［第３の実施形態］
各軸がモータで駆動されるロボットなどでは、モータの負荷である動作対象物とモータとの間には減速機が設けられることが一般的である。そして減速機には角度伝達誤差があり、この角度伝達誤差に起因して振動が発生することがある。この振動は、動作対象物を含む機械系に伝わる。また動作対象物を含む機械系は、特定の振動数すなわち固有振動数で振動しやすいという性質を有する。その結果、減速機の角度伝達誤差に起因する振動の周波数が機械系の固有振動数と一致するかそれに近い値となると、減速機側からの振動に対して動作対象物を含む機械系が共振する。角度伝達誤差に起因する振動の周波数は、一般的に、減速機の入力軸の回転数、すなわちモータの回転数に比例する。その一方で、機械系の固有振動数は、モータの回転数などに依存せずにほぼ一定である、したがって、機械系での共振は、モータの回転数がある範囲にあるときに発生することになる。機械系が共振した状態では、トルク指令値や位置偏差も大きく変動し、変動の周波数は共振の周波数と一致する。共振が発生すると、バンドパスフィルタ処理を行っているとしても共振の影響がバンドパスフィルタ処理後の信号にも及ぶから、上述した各実施形態の衝突検出方法により衝突発生を検出しようとするときに、実際には衝突が発生していないにも関わらず、衝突発生と誤検出することがある。そこで第３の実施形態では、このような誤検出の発生を防ぐための構成について説明する。

図８（ａ）は、ロボット１０に接続する制御装置４０の構成を示すブロック図であり、第３の実施形態の衝突検出装置４２を説明している。図８（ａ）に示す制御装置４０は、第１の実施形態での制御装置４０と同様のものであるが、衝突検出装置４２の構成がモータ３６の回転数すなわち回転速度に応じてコンパレータ４６における閾値を変化させるようにした点で、第１の実施形態での制御装置４０と異なっている。図８（ａ）に示す衝突検出装置４２では、コンパレータ４６における閾値を可変として、コンパレータ４６の閾値を制御するためのルックアップテーブル（ＬＵＴ）６１が設けられている。ルックアップテーブル６１は、モータ３６の回転速度ごとのコンパレータ４６での閾値を格納するものであり、モータ３６の回転速度が入力すると、それに対応した閾値をコンパレータ４６に出力する。モータ３６の回転速度は、外部から入力する位置指令値に基づいてモータ３６のサーボ制御を行なう制御部４１において常時算出されている速度指令値から知ることができるので、ルックアップテーブル６１には、回転速度として制御部４１から速度指令値が入力している。あるいは、エンコーダ３７がモータ３６の回転位置を示す値を常時出力しているので、この値を微分した値を求めて速度値とし、速度指令値の代わりにルックアップテーブル６１に与えてもよい。コンパレータ４６は、ルックアップテーブル６１から入力した閾値と絶対値演算部４５において算出された絶対値とを比較し、絶対値が閾値を上回っていれば、衝突の発生があったと判定する。

図８（ｂ）はルックアップテーブル６１の設定例を示している。図示される例は、モータ３６の最高速度に対する百分率でモータ３６の回転速度が表されるとして、モータ３６の速度が３４％から４０％までの範囲（共振発生速度）において減速機３８の角度伝達誤差による振動に対して負荷３９を含む機械系が共振するとしたときの、ルックアップテーブル６１の内容を示している。横軸がモータ３６の回転速度を示し、縦軸がコンパレータ４６において用いられる閾値を示している。閾値は、コンパレータ４６で用いられる電圧単位で示されている。モータ３６の最高速度とは、ロボット１０において軸ごとにその軸のモータ３６について許容されるものとして定められている最大の回転速度のことをいう。モータ３６の回転数と減速機３８の角度伝達誤差に起因する振動の振動数との関係は減速機３８の種類になどによって定まり、また、負荷３９を含む機械系の固有振動数は公知の方法によって求めることができるから、共振発生速度は事前に知ることができる。

図８（ｂ）に示す例では、モータ３６の速度が共振発生速度から大きく離れているとき（速度が最大速度の０～２４％あるいは５０～１００％であるとき）は、閾値の値は４００であり、これは、第１の実施形態で説明した場合において用いられる閾値と同じ値である。これに対して、モータ３６の速度が共振回転速度の範囲内であるといきは、閾値の値は８００である。そして、最大速度の２４％から３４％の速度範囲では、速度が大きくなるにつれて閾値は４００から８００へと変化し、最大速度の４０％から５０％の速度範囲では、速度が大きくなるにつれて閾値は８００から４００へと変化する。このように設定されたルックアップテーブル６１を用いてコンパレータ４６の閾値を変化させることにより、共振発生速度ではコンパレータ４６における衝突検出感度が小さくなって、負荷３９を含む機械系の共振による誤検出の発生を抑制することができる。

減速機３８の角度伝達誤差による振動の周波数が、負荷３９を含む機械系の固有振動数の高調波成分と共振し、それにより衝突検出における誤検出が起こることもある。そのような場合には、高調波の周波数に対応するモータ３６の回転速度の領域でも、コンパレータ４６の閾値を大きくしてその領域での衝突検出感度を小さくすることができる。例えば図８（ｂ）に示す例では、機械系の固有振動数に対応するモータ速度（共振発生速度）が最高速度の３４％から４０％であるので、２次高調波に対応するモータ速度は例えば最高速度の６８％から８０％となる。したがって、最高速度の６８％から８０％の速度範囲においても閾値が大きくなるようにルックアップテーブル６１を設定することができる。

負荷３９を含む機械系の共振によって衝突検知に誤検出が起こることを抑制する方法は、モータ３６の回転速度に応じてコンパレータ４６の閾値を変化させる方法に限定されるものではない。図９は、ロボット１０に接続する制御装置４０の構成を示すブロック図であり、第３の実施形態の衝突検出装置４２の別の例を説明している。図９に示す制御装置４０は、第１の実施形態での制御装置４０と同様のものであるが、バンドパスフィルタ４４と絶対値演算部４５の間に、負荷３９を含む機械系の固有振動数の周波数成分を減衰させるノッチフィルタ６２が設けられている点で、図１に示すものとは異なっている。機械系の固有振動数が例えば６Ｈｚであれば、ノッチフィルタ６２の中心周波数も６Ｈｚに設定される。ノッチフィルタ６２のフィルタ形式は任意であるが、２次のフィルタをノッチフィルタ６２に用いることが好ましい。図９に示す衝突検出装置４２によれば、機械系において共振が発生したとしてもその周波数成分の信号が減衰するので、衝突検出における誤検出の発生を抑制することができる。機械系の固有振動数の高調波においても共振が起きる場合には、基本波すなわち機械系の固有振動数そのものに対するノッチフィルタ６２に加え、高調波に対応する成分を減衰させるノッチフィルタを追加してもよい。

第３の実施形態においては、図８（ａ）に示したコンパレータ４６における閾値をモータ速度によって変化させる手法と、図９に示した機械系の固有振動数の周波数成分を除去するノッチフィルタ６２を設ける手法とを併用することもできる。図１０は、ロボット１０に接続する制御装置４０の構成を示すブロック図であり、図８（ａ）に示す手法と図９に示す手法とを併用する衝突検出装置４２を説明している。図１０に示す衝突検出装置４２は、図８（ａ）に示した衝突検出装置において、バンドパスフィルタ４４と絶対値演算部４５の間に、さらにノッチフィルタ６２を設けたものである。回転速度に応じてコンパレータ４６の閾値を変化させるとともに、機械系の固有振動数に対応するノッチフィルタ６２を設けることにより、負荷３９を含む機械系の共振によって衝突検出の誤検出が発生することをより確実に抑制することができる。

以上の説明では第１の実施形態の衝突検出装置４２において、コンパレータ４６の閾値をモータ速度によって変化させたり、機械系の固有振動数の周波数成分を減衰させるノッチフィルタ６２を設けたりしているが、第２の実施形態の衝突検出装置４２においても同様の手法を採用することにより、衝突検出における誤検出が発生することを抑制することができる。図１１及び図１２は、いずれもロボット１０に接続する制御装置４０の構成を示すブロック図であり、それぞれ、第３の実施形態の衝突検出装置４２のさらに別の例を示している。図１１に示す衝突検出装置４２は、図６に示す第２の実施形態の衝突検出装置４２において、上述と同様にルックアップテーブル６１を設け、モータ速度に応じてコンパレータ４６の閾値を変化できるようにしたものである。図１２に示す衝突検出装置４２は、図６に示す第２の実施形態の衝突検出装置４２において、バンドパスフィルタ４４と絶対値演算部４５の間にノッチフィルタ６２を設け、機械系の固有振動数の周波数成分を減衰させることができるようにしたものである。

以上説明した第３の実施形態では、減速機３８の角度伝達誤差に起因する振動などによって減速機３８から負荷３９までの機械系が共振する場合においても、この共振によって衝突検出における誤検出が発生することを抑制することができる。

以上、本発明に基づく衝突検出方法について、ロボット１０における衝突の発生を検出する場合を例に挙げて説明したが、本発明に基づく衝突検出方法は、動作対象物がサーボモータによって駆動されるものである限り、ロボット以外の動作対象物における衝突の発生の検出に広く用いることができる。

１０…ロボット；１１Ａ，１１Ｂ…第１アーム；１２Ａ，１２Ｂ…第２アーム；１３Ａ，１３Ｂ…ハンド；２１…レール；２２…基台；２３…回転台；２４…昇降機構；２４Ａ…固定部；２４Ｂ…移動部；２５…カバー；２６Ａ，２６Ｂ…アーム支持部；３１…回転軸；３２…共通軸；３３Ａ，３３Ｂ…軸；３４Ａ，３４Ｂ…手首軸；３６…モータ；３７エンコーダ；３８…減速機；３９…負荷；４０…制御装置；４１…制御部；４２…衝突検出装置；４３…減算器；４４…バンドパスフィルタ；４４Ｌ…ローパスフィルタ；４４Ｈ…ハイパスフィルタ；４５…絶対値演算部；４６…コンパレータ；４７…警報処理部；４８…ＲＭＳ演算部；４８Ａ…二乗演算部；４８Ｂ…積算部；４８Ｃ…平方根演算部；５０…ワーク；６１…ルックアップテーブル（ＬＵＴ）；６２…ノッチフィルタ。

Claims (11)

1. サーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出方法であって、
   前記サーボモータに対するトルク指令値に対してバンドパスフィルタ処理を行い、
   前記バンドパスフィルタ処理によって得られる信号の振幅に基づく値と閾値とを比較して、前記動作対象物における衝突の発生の有無を判定し、
   前記動作対象物を含む機械系の共振が発生する速度領域において他の速度領域に比べて前記閾値が大きくなるように、前記サーボモータの速度に応じて前記閾値を変化させる、衝突検出方法。
2. サーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出方法であって、
   前記サーボモータに対するトルク指令値に対してバンドパスフィルタ処理を行い、
   前記バンドパスフィルタ処理によって得られる信号に対し、前記動作対象物を含む機械系の固有振動数の成分を減衰させるノッチフィルタ処理を行い、前記ノッチフィルタ処理を行ったのちの信号の振幅に基づく値と閾値とを比較して、前記動作対象物における衝突の発生の有無を判定し、
   前記動作対象物を含む機械系の共振が発生する速度領域において他の速度領域に比べて前記閾値が大きくなるように、前記サーボモータの速度に応じて前記閾値を変化させる、衝突検出方法。
3. 前記バンドパスフィルタ処理は、２次のバタワースフィルタ、３次のバタワースフィルタ及び３次のベッセルフィルタのいずれかによるバンドパスフィルタ処理である、請求項１または２に記載の衝突検出方法。
4. 前記振幅に基づく値は、前記バンドパスフィルタ処理によって得られる信号の絶対値、二乗平均値及び二乗平均平方根値のいずれかである、請求項１乃至３のいずれか１項に記載の衝突検出方法。
5. 前記動作対象物はロボットのアームまたはハンドである、請求項１乃至４のいずれか１項に記載の衝突検出方法。
6. サーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出装置であって、
   前記サーボモータに対するトルク指令値に対してバンドパスフィルタ処理を行うバンドパスフィルタと、
   前記バンドパスフィルタから出力される信号の振幅に基づく値と閾値とを比較して前記動作対象物における衝突の発生の有無を判定する判定手段と、
   前記動作対象物を含む機械系の共振が発生する速度領域において他の速度領域に比べて前記閾値が大きくなるように、前記サーボモータの速度に応じて前記閾値を変化させる閾値変化手段と、
   を有する、衝突検出装置。
7. サーボモータによって駆動される動作対象物における衝突の発生を検出する衝突検出装置であって、
   前記サーボモータに対するトルク指令値に対してバンドパスフィルタ処理を行うバンドパスフィルタと、
   前記バンドパスフィルタから出力される信号が供給されて、前記動作対象物を含む機械系の固有振動数の成分を減衰させるノッチフィルタと、
   前記ノッチフィルタから出力される信号の振幅に基づく値と閾値とを比較して前記動作対象物における衝突の発生の有無を判定する判定手段と、
   前記動作対象物を含む機械系の共振が発生する速度領域において他の速度領域に比べて前記閾値が大きくなるように、前記サーボモータの速度に応じて前記閾値を変化させる閾値変化手段と、
   を有する、衝突検出装置。
8. 前記バンドパスフィルタは、２次のバタワースフィルタ、３次のバタワースフィルタ及び３次のベッセルフィルタのいずれかからなる、請求項６または７に記載の衝突検出装置。
9. 前記閾値変化手段は、前記サーボモータの速度が入力して前記入力した速度に応じた前記閾値を出力するルックアップテーブルで構成されている、請求項６乃至８のいずれか１項に記載の衝突検出装置。
10. 前記振幅に基づく値は、前記バンドパスフィルタから出力される信号の絶対値、二乗平均値及び二乗平均平方根値のいずれかである、請求項６乃至９のいずれか１項に記載の衝突検出装置。
11. 外部から与えられる指令値に基づいて前記サーボモータの制御を実行する制御装置の内部に設けられている、請求項６乃至１０のいずれか１項に記載の衝突検出装置。