特開 2024-14341 ロボットの衝突検出方法及びロボットの衝突検出装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024014341

(43)【公開日】2024-02-01

(54)【発明の名称】ロボットの衝突検出方法及びロボットの衝突検出装置

(51)【国際特許分類】

   B25J 19/06 20060101AFI20240125BHJP

   H01L 21/677 20060101ALI20240125BHJP

【ＦＩ】

B25J19/06

H01L21/68 A

【審査請求】未請求

【請求項の数】10

【出願形態】ＯＬ

(21)【出願番号】P 2022117098

(22)【出願日】2022-07-22

(71)【出願人】

【識別番号】000002233

【氏名又は名称】ニデックインスツルメンツ株式会社

(74)【代理人】

【識別番号】100123788

【弁理士】

【氏名又は名称】宮崎 昭夫

(74)【代理人】

【識別番号】100127454

【弁理士】

【氏名又は名称】緒方 雅昭

(72)【発明者】

【氏名】猪股 徹也

(72)【発明者】

【氏名】王 ▲カク▼

【テーマコード（参考）】

3C707

5F131

【Ｆターム（参考）】

3C707AS24

3C707HS27

3C707KS21

3C707KS22

3C707KS35

3C707KX10

3C707LT01

3C707MS07

3C707NS13

5F131AA02

5F131CA53

5F131DB02

5F131DB52

5F131DB76

5F131DB82

5F131DD42

5F131DD79

5F131DD82

(57)【要約】

【課題】ロボットが低速で運動しているときであっても、小さな演算負荷量で、ロボットにおける衝突の発生を速やかかつ確実に検出する。
【解決手段】各軸がモータによって駆動されるロボット１０における衝突の発生を検出する衝突検出装置は、閾値（衝突検出リミット値）を発生する閾値発生部５３と、閾値とモータ１１のトルク値とを比較して、トルク値が閾値を上回ったときに衝突が発生したと判定する比較部５４と、ステージごとかつ軸ごとに全体速度ごとの閾値をあらかじめ格納した衝突検出リミットテーブルを格納する記憶部５５と、を有する。ロボット１０がステージごとにそのステージの進入位置とステージ位置との間をＣＰ制御によって移動するときに、閾値発生部５３は、衝突検出リミットテーブルを参照して得られる閾値を比較部５４に供給する。全体速度は、ＣＰ制御により動作するロボット１０の全体としての速度である。
【選択図】図１

【特許請求の範囲】

【請求項1】

複数の軸を備えて各軸がそれぞれモータによって駆動されるロボットにおける衝突の発生を検出する衝突検出方法であって、
軸ごとに前記モータの速度に応じて定まる閾値と当該軸の前記モータのトルク値とを比較し、前記トルク値が前記閾値を上回ったときに衝突が発生したと判定する衝突判定工程を有し、
前記ロボットが複数のステージの間でワークを搬送するロボットであって前記ステージごとに当該ステージの進入位置とステージ位置との間を前記ロボットが連続経路制御によって移動するときに、前記衝突判定工程において、ステージごとかつ軸ごとに全体速度ごとの前記閾値をあらかじめ格納した衝突検出リミットテーブルを参照して得られる前記閾値が使用され、
前記全体速度は、前記連続経路制御により動作する前記ロボットの全体としての速度である、衝突検出方法。

【請求項2】

前記衝突検出リミットテーブルにおいて、全体速度が速度ゼロから前記連続経路制御で規定された最大速度までの範囲で複数の連続経路制御用速度領域に区分され、前記連続経路制御用速度領域ごとに前記閾値が単一の値として定められている、請求項１に記載の衝突検出方法。

【請求項3】

前記閾値は、前記ロボットの前記連続経路制御でのティーチングが完了したときに、前記モータのトルク波形における最大値に基づいて決定されて前記衝突検出リミットテーブルに格納される、請求項１または２に記載の衝突検出方法。

【請求項4】

前記ロボットは、待機位置と前記進入位置との間をポイント・ツー・ポイント制御で移動し、
前記ポイント・ツー・ポイント制御で移動しているときは、前記モータの速度が低いほど小さく前記モータの速度が大きいほど大きくなるよう前記複数の軸の各々に対する速度割合に応じて軸ごとに定められた速度に基づいて、当該軸に対する前記閾値が定められる、請求項１または２に記載の衝突検出方法。

【請求項5】

速度ゼロから前記モータの最大速度までの範囲が複数のポイント・ツー・ポイント制御用速度領域に区分され、前記ポイント・ツー・ポイント制御用速度領域ごとに前記ポイント・ツー・ポイント制御のときに使用される前記閾値が単一の値として定められている、請求項４に記載の衝突検出方法。

【請求項6】

複数の軸を備えて各軸がそれぞれモータによって駆動されるロボットにおける衝突の発生を検出する衝突検出装置であって、
軸ごとに前記モータの速度に応じて定まる閾値を発生する閾値発生部と、
前記閾値発生部から供給された前記閾値と前記モータのトルク値とを比較して、前記トルク値が前記供給された閾値を上回ったときに衝突が発生したと判定する比較部と、
ステージごとかつ軸ごとに全体速度ごとの前記閾値をあらかじめ格納した衝突検出リミットテーブルを格納する記憶部と、
を有し、
前記ロボットが複数のステージの間でワークを搬送するロボットであって前記ステージごとに当該ステージの進入位置とステージ位置との間を前記ロボットが連続経路制御によって移動するときに、前記閾値発生部は、前記衝突検出リミットテーブルを参照して得られる前記閾値を前記比較部に供給し、
前記全体速度は、前記連続経路制御により動作する前記ロボットの全体としての速度である、衝突検出装置。

【請求項7】

前記衝突検出リミットテーブルにおいて、全体速度が速度ゼロから前記連続経路制御で規定された最大速度までの範囲で複数の連続経路制御用速度領域に区分され、前記連続経路制御用速度領域ごとに前記閾値が単一の値として定められている、請求項６に記載の衝突検出装置。

【請求項8】

前記モータのトルク波形における最大値に基づいて前記閾値を決定して前記衝突検出リミットテーブルに格納するテーブル生成部をさらに有する、請求項６または７に記載の衝突検出装置。

【請求項9】

前記ロボットは、待機位置と前記進入位置との間をポイント・ツー・ポイント制御で移動し、
前記ポイント・ツー・ポイント制御で移動しているときに前記閾値発生部が発生する前記閾値は、前記モータの速度が低いほど小さく前記モータの速度が大きいほど大きくなるよう前記複数の軸の各々に対する速度割合に応じて軸ごとに定められた速度に基づいて定められたものである、請求項６または７に記載の衝突検出装置。

【請求項10】

速度ゼロから前記モータの最大速度までの範囲が複数のポイント・ツー・ポイント制御用速度領域に区分され、前記ポイント・ツー・ポイント制御用速度領域ごとに前記ポイント・ツー・ポイント制御のときに使用される前記閾値が単一の値として定められている、請求項９に記載の衝突検出装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ロボットにおける衝突の発生を検出する方法及び装置に関する。

【背景技術】

【0002】

ワークの搬送などに用いられるロボットは、動作中にそのアームなどが周囲の物体と干渉して衝突することがある。動作中に衝突が起きたときは、直ちにそれを検出してロボットの動作を緊急に停止させるなどの処理を行う必要がある。ロボットにおける衝突の検出には、そのアームやアーム先端のハンドなどに取り付けられた衝突センサを用いることができる。しかしながら衝突センサを設けることは、コストの上昇をもたらす。さらに、衝突センサの質量がロボットの動特性に悪影響を及ぼすことがあり、また、ロボットの構造によっては衝突センサへの配線を設けることが困難な場合もある。衝突センサによらずに衝突を検出する方法として、ロボットの各軸のサーボモータにおけるトルクを監視する方法がある。衝突が起きてロボットの動きが阻害されると、対応する軸のサーボモータのトルクが急激に上昇するので、閾値との比較によりトルクの上昇を検出した場合、衝突が発生したと判断することができる。この方法では、外部からの指令に応じてロボットを正常に動作させているときであっても指令の内容によってはトルクが大きく上昇することがあるので、衝突が発生したと判定するための閾値すなわち衝突検出リミット値を適切に設定する必要がある。平常運転において発生し得るトルクよりも小さな衝突検出リミット値を設定すると、平常運転時において衝突の発生を誤検出することになる。一般にトルクはモータの速度が大きいほど大きくなるから、誤検出を防ぐためには、モータが最大速度で駆動されるときトルクよりも大きな値に衝突検出リミット値を設定する必要がある。

【0003】

最大速度での駆動に必要なトルクよりも大きな値に衝突検出リミット値を設定したときは、モータの速度が小さいときに衝突の検出を的確に行うことができない、という課題を生ずる。低速であればモータのトルクも小さく、衝突や干渉があったときのトルクの上昇も小さい。したがって、最大速度に対応した衝突検出リミット値を使用したときは、程度の小さな衝突や干渉を検出できない恐れがある。あるいは、衝突や干渉がある程度進行してから初めてその衝突や干渉を検出する、すなわち検出にタイムラグが生ずるおそれがある。

【0004】

ロボットでの衝突発生を速やかかつ確実に検出できる技術として特許文献１は、位置指令、速度指令及び加速度指令の少なくとも１つから演算した必要駆動トルク指令要素と各軸を駆動するモータの位置、速度及び加速度の少なくとも１つとから、ロボットの運動方程式の再計算を行うことなく必要駆動トルクを算出し、算出された必要駆動トルクと各軸のモータの電流とを比較することにより衝突の発生の判別を行うことを開示している。特許文献２は、ワークを保持しているか否かに応じて駆動トルクが変化するという状況において衝突の発生を確実に検出するために、ワークの荷重がロボットに伝達されない状態とワークの荷重がロボットに伝達される状態との間の状態遷移を認識してどちらの状態であるか応じて異なる閾値を使用し、外乱推定値をこの閾値と比較して衝突の検出を行うことを開示している。特許文献３は、逆動力学計算を行って外乱を推定して衝突の発生を検出するときに、減速機のばね成分による振動のために衝突検出の精度が低下することを防ぐために、予め設定されている所定値よりもロボット動作の指令加速度が大きい場合に、衝突検出における閾値を大きくして検出感度を低下させることを開示している。

【0005】

衝突検出の精度向上に関するものではないが、特許文献４は、人間である作業者とロボットとが共存して作業を行うために、ロボットアームが移動する空間を、作業者に近接する空間である低速動作領域とそれ以外の空間である高速動作領域とに分け、ロボットアームの衝突を検出するための検知感度を高速動作領域よりも低速動作領域で高くすることを開示している。

【先行技術文献】

【特許文献】

【0006】

【特許文献1】特開２００３－２５２７２号公報

【特許文献2】特開２０１６－１６４９０号公報

【特許文献3】特開２００６－１１６６５０号公報

【特許文献4】国際公開第２０１６／１０３３０８号

【発明の概要】

【発明が解決しようとする課題】

【0007】

ロボットにおける衝突検出の精度を高める技術として特許文献１に記載されたものは必要駆動トルクの算出が必要であり、特許文献２に記載されたものは外乱の推定値の算出が必要であり、特許文献３に記載されたものは逆動力学演算を行って外力を算出する必要がある。したがって特許文献１－３に記載された技術は、衝突検出のために必要な演算負荷が重い、という課題を有する。

【0008】

本発明の目的は、演算負荷量が小さく、ロボットが低速で運動しているときであっても衝突の発生を速やかかつ確実に検出できる衝突検出方法及び衝突検出装置を提供することにある。

【課題を解決するための手段】

【0009】

本発明の一態様において、複数の軸を備えて各軸がそれぞれモータによって駆動されるロボットにおける衝突の発生を検出する衝突検出方法は、軸ごとにモータの速度に応じて定まる閾値とその軸のモータのトルク値とを比較し、トルク値が閾値を上回ったときに衝突が発生したと判定する衝突判定工程を有する。ロボットが複数のステージの間でワークを搬送するロボットであってステージごとにそのステージの進入位置とステージ位置との間をロボットが連続経路（ＣＰ）制御によって移動するときに、衝突判定工程において、ステージごとかつ軸ごとに全体速度ごとの閾値をあらかじめ格納した衝突検出リミットテーブルを参照して得られる閾値が使用され、全体速度は、ＣＰ制御により動作するロボットの全体としての速度である。

【0010】

上記態様の衝突検出方法によれば、実際にロボットを動作させないと軸ごとのモータの速度の変化を知ることができないＣＰ制御を行なう場合において、ステージごとかつ軸ごとに全体速度ごとの閾値をあらかじめ格納した衝突検出リミットテーブルを用意しておき、ＣＰ制御でロボットを実際に移動させるときには指令された全体速度によって衝突検出リミットテーブルを参照して閾値である衝突検出リミット値を得ることによって、ＣＰ制御によってロボットをステージにアクセスさせる際に、より最適な衝突検出を行うことができる。

【0011】

一態様の衝突検出方法では、衝突検出リミットテーブルにおいて、全体速度が速度ゼロからＣＰ制御で規定された最大速度までの範囲で複数のＣＰ用速度領域に区分され、ＣＰ用速度領域ごとに閾値が単一の値として定められていることが好ましい。ＣＰ用速度領域ごとに閾値を単一の値として定めることにより、衝突検出リミットテーブルのサイズを小さくし、作成時間を短縮できる。

【0012】

一態様の衝突検出方法では、閾値は、ロボットのＣＰ制御でのティーチングが完了したときに、モータのトルク波形における最大値に基づいて決定されて衝突検出リミットテーブルに格納されるようにすることが好ましい。ティーチング完了時に衝突検出リミットテーブルを作成することにより、実際にロボットを稼働させるときに確実に衝突検出を行うことができる。

【0013】

一態様の衝突検出方法では、ロボットは、待機位置と進入位置との間をポイント・ツー・ポイント（ＰＴＰ）制御で移動する。ＰＴＰ制御で移動しているときは、モータの速度が低いほど小さくモータの速度が大きいほど大きくなるよう複数の軸の各々に対する速度割合に応じて軸ごとに定められた速度に基づいて、その軸に対する閾値が定められるようにすることが好ましい。このように構成することにより、ＰＴＰ制御でロボットを移動させるときにおいて、より適切な閾値を用いて衝突発生の検出を行うことができる。この場合、速度ゼロからモータの最大速度までの範囲が複数のＰＴＰ制御用速度領域に区分され、ＰＴＰ制御用速度領域ごとにＰＴＰ制御のときに使用される閾値が単一の値として定められるようにすることが好ましい。これにより、閾値発生のための演算負荷をさらに小さくできる。

【0014】

本発明の別の態様において、複数の軸を備えて各軸がそれぞれモータによって駆動されるロボットにおける衝突の発生を検出する衝突検出装置は、軸ごとにモータの速度に応じて定まる閾値を発生する閾値発生部と、閾値発生部から供給された閾値とモータのトルク値とを比較して、トルク値が供給された閾値を上回ったときに衝突が発生したと判定する比較部と、ステージごとかつ軸ごとに全体速度ごとの閾値をあらかじめ格納した衝突検出リミットテーブルを格納する記憶部と、を有する。この衝突検出装置では、ロボットが複数のステージの間でワークを搬送するロボットであってステージごとにそのステージの進入位置とステージ位置との間をロボットがＣＰ制御によって移動するときに、閾値発生部は、衝突検出リミットテーブルを参照して得られる閾値を比較部に供給し、全体速度は、ＣＰ制御により動作するロボットの全体としての速度である。

【0015】

上記態様の衝突検出装置では、実際にロボットを動作させないと軸ごとのモータの速度の変化を知ることができないＣＰ制御を行なう場合において、ステージごとかつ軸ごとに全体速度ごとの閾値をあらかじめ格納した衝突検出リミットテーブルを用意して記憶部に記憶させておき、ＣＰ制御でロボットを実際に移動させるときには指令された全体速度により衝突検出リミットテーブルを参照して閾値である衝突検出リミット値を得ることによって、ＣＰ制御によってロボットをステージにアクセスさせる際に、より最適な衝突検出を行うことができるようになる。

【0016】

一態様の衝突検出装置では、衝突検出リミットテーブルにおいて、全体速度が速度ゼロからＣＰ制御で規定された最大速度までの範囲で複数のＣＰ制御用速度領域に区分され、ＣＰ制御用速度領域ごとに記閾値が単一の値として定められていることが好ましい。ＣＰ制御用速度領域ごとに閾値を単一の値として定めることにより、衝突検出リミットテーブルのサイズを小さくし、作成時間を短縮することができる。また、モータのトルク波形における最大値に基づいて閾値を決定して衝突検出リミットテーブルに格納するテーブル生成部が設けられていてもよい。このようなテーブル作成部を設けることにより、衝突検出リミットテーブルの作成の処理を簡単に実行できるようになる。

【0017】

一態様の衝突検出装置では、ロボットは、待機位置と進入位置との間をＰＴＰ制御で移動し、ＰＴＰ制御で移動しているときに閾値発生部が発生する閾値は、モータの速度が低いほど小さくモータの速度が大きいほど大きくなるよう複数の軸の各々に対する速度割合に応じて軸ごとに定められた速度に基づいて定められたものであってもよい。このように構成することにより、ＰＴＰ制御でロボットを移動させるときにおいて、より適切な閾値を用いて衝突発生の検出を行うことができる。この場合、速度ゼロからモータの最大速度までの範囲が複数のＰＴＰ制御用速度領域に区分され、ＰＴＰ制御用速度領域ごとにＰＴＰ制御のときに使用される閾値が単一の値として定められるようにすることが好ましい。これにより、閾値発生のための演算負荷をさらに小さくできる。

【発明の効果】

【0018】

本発明によれば、小さな演算負荷量で、ロボットが低速で運動しているときであっても衝突の発生を速やかかつ確実に検出できるようになる。

【図面の簡単な説明】

【0019】

【図1】本発明の実施の一形態の衝突検出方法が適用されるロボットシステムの一例を示すブロック図である。

【図2】トルク値に基づく衝突検出の原理を示す図である。

【図3】本発明に基づく衝突検出の原理を説明する図である。

【図4】搬送用のロボットの構成の一例を示す概略断面図である。

【図5】ロボットの動作例を示す概略平面図である。

【図6】ＰＴＰ制御を説明する図である。

【図7】ＣＰ制御を説明する図である。

【図8】衝突検出リミットテーブルの内容の一例を示す図である。

【発明を実施するための形態】

【0020】

次に、本発明の実施の形態について、図面を参照して説明する。本発明に基づく衝突検出方法は、搬送用ロボットなどのロボットにおける衝突の発生を検出するものである。ここでいう衝突には、ロボットのアームやハンドが他の物体に文字通り衝突する場合のほかに、ロボットの動作に伴ってアームやハンドの移動が他の物体によって影響を受けあるいは妨げられる干渉も含まれるものとする。まず、本発明の実施の一形態の衝突検出方法が適用されるロボットシステムについて説明する。図１は、そのようなロボットシステムの構成の一例を示している。

【0021】

ロボットシステムは、半導体ウエハの搬送などに用いられるロボット１０と、外部からの指令に基づいてロボット１０を駆動し制御するロボット制御装置（ロボットコントローラ）５０とから構成されている。ロボット１０は、そのアームやハンドなどを駆動するモータ１１と、モータ１１に機械的に接続してモータ１１の回転位置を検出するエンコーダ
１２とを備えている。ロボット１０は、通常、複数の軸を有して軸ごとにモータ１１を備えるが、図１では、説明のため、ロボット１０において１軸分のモータ１１とそのモータ１１に接続するエンコーダ１２が描かれている。

【0022】

ロボット制御装置５０は、外部から入力する指令に基づいてロボット１０の軌道を算出し、ロボット１０の各軸のモータ１１に対する位置指令値を出力する演算部５１と、演算部５１から出力される位置指令値に基づいてモータ１１を駆動する駆動部５２を備えている。エンコーダ１２からはモータ１１の回転位置を示す信号が出力しており、この信号が駆動部５２にフィードバックされることによって、駆動部５２は、モータ１１のサーボ制御を実行する。ロボット１０のティーチング（教示）を行うときはロボット制御装置５０に対してティーチングペンダントが接続されるが、ティーチングペンダントからの指令も外部指令として演算部５１に入力する。ここで述べた演算部５１及び駆動部５２を備える構成は、産業用ロボットの制御に用いられる一般的なロボット制御装置の構成と同じである。

【0023】

図１に示すロボット制御装置５０は、ロボット１０における衝突の発生を検出するために、さらに、閾値である衝突検出リミット値を出力する閾値発生部５３と、モータ１１のトルクの値と閾値発生部５３から供給された衝突検出リミット値（閾値）とを比較する比較部５４と、ロボット１０が後述するＣＰ制御により動作しているときに閾値発生部５３によって参照される衝突検出リミットテーブルが格納される記憶部５５と、衝突検出リミットテーブルを生成するテーブル生成部５６とを備えている。比較部５４は、例えばコンパレータ回路によって構成されており、トルク値が衝突検出リミット値を上回ったときに、ロボット１０において衝突が発生したと判定する。比較部５４での判定結果は衝突検出結果として外部に出力される。衝突発生時にロボット１０を緊急停止させるためなどに、この衝突検出結果を演算部５１に入力させてもよい。本発明に基づく衝突検出方法において、モータ１１のトルクの値としては、モータ１１のサーボ制御のために駆動部５２の内部で計算されるトルク指令値を用いてもよいし、モータ１１の電流値そのものを使用してもよいし、モータ１１に関して実際に測定されたトルク値を用いてもよい。モータ１１に関して実際に測定されたトルク値は、何らかの実測値に基づいて逆動力学計算やモデル演算を行って得られた値であってもよい。閾値発生部５３と比較部５４とによって、本発明に基づく衝突検出装置が構成される。閾値発生部５３、テーブル生成部５６及び衝突検出リミットテーブルの詳細については後述する。

【0024】

本発明に基づく衝突判定方法は、モータ１１のトルク値と閾値である衝突検出リミット値とを比較することに基づくものである。図２は、この衝突判定方法の原理を説明する図であり、横軸が時間であって縦軸がモータ１１のトルクである。図２（ａ）に示すように、平常動作時のモータ１１のトルクは、時々刻々と変化するが、一定の範囲内に収まっている。ここでロボット１０の例えばアームが、モータ１１によって駆動されているときに時刻Ｐにおいて他の物体と衝突すると、アームの動きが妨げられるから、この妨げる力に対抗するようにモータ１１のトルク値が急上昇する。急上昇したトルクが予め定められている衝突検出リミット値を超えたことを検出することにより、衝突の発生を検出できる。モータ１１のトルクは、図２（ｂ）に示すように、そのモータ１１が低速で動いているのか高速で動いているのかによって異なる。低速動作中はトルクは相対的に小さく、高速動作中はトルクは相対的に大きい。単一の衝突検出リミット値を用いてロボット１０における衝突の発生を検出しようとするときは、衝突検出リミット値は、平常動作時に最大速度で回転しているモータ１１に起こり得るトルクよりも大きな値とする必要がある。

【0025】

高速動作時のトルクに基づいて衝突検出リミット値を設定すると、この衝突検出リミット値は、モータ１１が低速で動作している場合には大きすぎる値であって、ロボット１０での衝突の発生を適切に検出するためには不適切である。図３は図２（ａ）と同様の図であるが、モータ１１が比較的低速で動作しているときのトルクの変化を示している。図に示される衝突検出リミット値は、モータ１１の最大速度に基づいて設定されたものである。時刻Ｐにおいて衝突が発生すると、トルク値は急上昇するが、衝突検出リミット値には至らないので、その時点では衝突の発生は検出されない。そのままトルクは上昇を続け、時刻Ｑにおいてようやく衝突検出リミット値に到達するので、時刻Ｑにおいて衝突の発生が検出されることとなる。時刻Ｐから時刻Ｑまでの時間は、検出のタイムラグに相当する。検出にタイムラグが生じると、ロボットが被衝突物に対してより大きな力を作用させる、あるいはロボットが被衝突物に対してさらに食い込むように動くこととなるので、衝突検出におけるタイムラグは、例えば安全確保の上で、極めて好ましくない。また、衝突の程度が小さい場合、例えば、アームが小さな物体に衝突したが、衝突の結果、物体がアームの軌道から外れる位置に移動したのでアームが引き続き動き続けることができる場合には、衝突によるトルクの上昇量が小さくて衝突検出リミット値まで到達しないことがあり、衝突の発生が検出されないことがある。規模が小さい衝突とはいえ検出できないことは、これも安全確保の上で極めて好ましくない。

【0026】

本発明に基づく衝突検出方法では、ロボットが低速で運動しているときであっても衝突の発生を速やかかつ確実に検出できるようにするために、モータ１１の速度が小さいときには衝突検出リミット値が小さくなり、速度が大きいときには衝突検出リミット値が大きくなるように、モータ１１の速度に応じて閾値である衝突検出リミット値を変化させる。図３において矢印は、低速動作時には衝突検出リミット値を小さくすることを示しており、これにより、実際に衝突が発生した時刻Ｐにおいて、タイムラグなしに衝突発生を検出できるようになる。モータ１１の速度はエンコーダ１２からのモータ位置を示す信号を微分することによって得られるものであり、駆動部５２において常時算出されているから、閾値発生部５３は、駆動部５２からモータ速度を受け取って、そのモータ速度に応じて閾値である衝突検出リミット値を生成し、比較部５４に出力する。あるいは、演算部５１においてモータ１１の速度を計算しているのであれば、図において破線で示すように閾値発生部５３は、演算部５１からモータ速度を受け取ってもよい。実際にはモータ速度について、速度ゼロからモータ１１の最大速度までの範囲を複数の速度領域に区分し、区分された速度領域ごとに単一の値として衝突検出リミット値を設定することが好ましい。このように速度領域ごとに衝突検出リミット値を設定することとすると、ルックアップテーブルによって閾値発生部５３を実現することが可能になり、閾値発生部５３の構成を簡素なものとして衝突検出リミット値の算出のための演算量を削減することができる。

【0027】

表１は、従来の方法による衝突検出リミット値の例を示している。モータ１１の現在の速度が、そのモータ１１の最大速度を１００％とする百分率で表されているとして、従来の方法では、モータ速度が０％から１００％の間で衝突検出リミット値は一律に例えば２００ｃＮ・ｍと設定される。これに対して表２は、本発明に基づく衝突検出方法で用いられる衝突検出リミット値の一例を示している。ここに示す例では、モータ１１の速度範囲が、０％以上１０％以下、１０％超２０％以下、２０％超３０％以下のように、１０％刻みで１０の速度領域に区分されている。最も低速である速度領域、すなわち０％以上１０％以下の速度領域には８３ｃＮ・ｍが衝突検出リミット値として設定され、次の速度領域、すなわち１０％超２０％以下の速度領域には９６ｃＮ・ｍが設定され、速度が最も高い速度領域すなわち９０％超１００％以下の速度領域に対しては２００ｃＮ・Ｎが設定されている。

【0028】

【表1】

【0029】

【表2】

【0030】

速度領域ごとの衝突検出リミット値は、例えば、速度を変えてモータ１１を駆動し、そのときのトルク波形を計測し、そのときの最大トルクに適切な安全率を乗じることによって算出することができる。このようにして事前に算出されたモータ速度ごとの衝突検出リミット値は、閾値発生部５３に設定されるが、ルックアップテーブルとして記憶部５５に格納されてもよい。

【0031】

以上、ロボット１０における特定の１つの軸のモータ１１についての衝突検出リミット値について説明した。実際にはロボット１０には複数の軸が設けられており、軸ごとに衝突検出を行う必要があるから、軸ごとにその軸のモータ速度に応じて衝突検出リミット値を変化させる必要がある。以下、そのような例について説明する。図４は、複数の軸を有するロボット１０の一例を示している。図示されるロボット１０は、３リンク型の水平多関節型ロボットであり、半導体ウエハの搬送などに用いられるものである。ロボット１０は、基台２１と、基台２１に設けられた昇降部２２と、昇降部２２に基端側が取り付けられたリンク機構２３と、リンク機構２３の先端部に対して基端側が回転可能に取り付けられたアーム２６と、アーム２６の先端に回転可能に保持されてワークを保持する２つのハンド２７，２８とを備えている。ハンド２７は上側のハンドであり、ハンド２８は下側のハンドである。昇降部２２は、モータ１１Ｚにより駆動されて垂直方向（図示Ｚ方向）に沿って昇降する。ここでは説明のため、昇降部２２の昇降をＺ軸の動きと呼ぶこととする。したがって、モータ１１ＺはＺ軸のモータである。

【0032】

リンク機構２３は、その先端部すなわちアーム２６が取り付けられている位置の移動軌跡が直線となるように構成されており、基台２１側に位置して昇降部２２に回転可能に保持される基台側リンク２４と、アーム２６側に位置するアーム側リンク２５とを備え、両方のリンク２４，２５はリンク関節部Ｊによって互いに回転可能に連結されている。基台側リンク２４は、昇降部２２に連結され、昇降部２２に内蔵されたモータ１１Ａによって回転可能に保持されている。基台側リンク２４には、基台側プーリ２４ａ、アーム側プーリ２４ｂ及びベルト２４ｃが内蔵されており、ベルト２４ｃは基台側プーリ２４ａとアーム側プーリ２４ｂの間で架けわたされている。基台側プーリ２４ａとアーム側プーリ２４ｂとの径の比は２：１となっている。アーム側プーリ２４ｂはアーム側リンク２５に連結されており、基台側リンク２４が基台側プーリ２４ａの回転中心を中心として回転したとき、基台側プーリ２４ａとアーム側プーリ２４ｂとの回転角度比、すなわち基台側リンク２４とアーム側リンク２５との回転角度比は１：２となるように構成されている。さらに、基台側リンク２４とアーム側リンク２５の長さは等しい。その結果、モータ１１Ａを駆動することによってリンク機構２３の先端部が移動することとなるが、その移動軌跡は、所定の直線上に規制される。リンク機構２３を移動させる動きをＡ軸の動きと呼ぶ。したがってモータ１１ＡはＡ軸のモータである。

【0033】

アーム２６は、アーム側リンク２５の先端に連結されており、アーム側リンク２５に内蔵されたモータ１１Ｂによって回転可能に保持されている。モータ１１Ｂを駆動することによって、アーム２６は、その基端側すなわちリンク機構２３との接続位置を中心として回転する。アーム２６のこの動きをＢ軸の動きと呼ぶ。モータ１１ＢはＢ軸のモータである。ハンド２７，２８は、それぞれ、アーム２６に内蔵されたモータ１１Ｃ，１１Ｄによって回転可能に保持されており、モータ１１Ｃ，１１Ｄによって駆動されることによって、アーム２６の先端を中心として回転する。モータ１１Ｃによるハンド２７の動きをＣ軸の動きと呼び、モータ１１Ｄによるハンド２８の動きをＤ軸の動きと呼ぶ。モータ１１Ｃ，１１Ｄは、それぞれ、Ｃ軸のモータ、Ｄ軸のモータである。このように図４に示すロボット１０は、Ａ軸、Ｂ軸、Ｃ軸、Ｄ軸及びＺ軸からなる５軸のロボットであり、５個のモータ１１Ａ，１１Ｂ，１１Ｃ，１１Ｄ，１１Ｚを備えている。モータ１１Ａ，１１Ｂ，１１Ｃ，１１Ｄ，１１Ｚはロボット制御装置５０により一括して制御される。衝突検出のために、ロボット制御装置５０には、軸ごとに閾値発生部５３と比較部５４が設けられる。これに対し、記憶部５５及びテーブル生成部５６は、ロボット１０の複数の軸に対して共通に設けられる。

【0034】

ところで、教示データに基づきロボットの手先を目標位置に移動させるときの軌道制御の形態として、よく知られているように、ＰＴＰ（ポイント－ツー－ポイント；point-to-point）制御とＣＰ（連続経路；continuous path）制御とがある。ＰＴＰ制御は、一般に、ロボットに取り付けたツールやハンドの先端がとるべき軌道の始点と終点のみを指定してツールやハンドを移動させる制御であり、ＣＰ制御は、一般的には３次元空間での直線（場合によっては曲線）である経路を指定してその経路に沿ってツールやハンドの先端を動かす制御である。ＰＴＰ制御では、教示データにおいては始点と終点とは示されるが始点と終点との間でのロボットの経路は指定されず、特に２以上の軸を有するロボットでは、軸ごとに始点と終点との間でその軸がどれだけ動くべきかを決定した後は、軸ごとの移動量によって各軸が独立に動かされる。ＣＰ制御は、指定された経路からずれないように各瞬間ごとに各軸の制御を行うものであり、例えば、教示データで示される教示点間の動きを直線で補間する際に使用される。ＰＴＰ制御の方がＣＰ制御よりも高速でロボットを移動させることができるが、ロボットの経路が指定されないので、ＰＴＰ制御ではロボット周囲の壁面などの干渉がおこりやすい。これに対してＣＰ制御では、ロボットの経路を指定できるので、ＰＴＰ制御に比べて低速ではあるが、壁面などとの干渉を確実に防ぐことができる。

【0035】

図４に示すロボット１０は搬送用のロボットであるので、ワークの搬送のために、ある位置から別の位置に移動するように制御される。その際、ＰＴＰ制御とＣＰ制御とを組み合わせた形態でロボット１０は制御される。ＰＴＰ制御及びＣＰ制御に必要な演算は、ロボット制御装置５０において演算部５１が実行する。

【0036】

図５は、ワークとして半導体ウエハを搬送するときのロボット１０の動作例を示している。半導体ウエハがワーク６０であるときは、図５に示すように、複数のステージ６１の間でワーク６０を搬送するためにロボット１０が使用される。ここではワーク６０を格納するカセットや、ワーク６０を処理するワーク処理装置を総称してステージ６１と称している。複数のステージ６１は、ロボット１０を取り囲む空間である作業領域に面するように配置しており、ステージ６１の開口を介してそのステージ６１の内部にロボット１０のハンド２７，２８を差し入れることができるように構成されている。作業領域内に設置されているロボット１０は、通常、図５において（ａ）で示す待機位置において、そのリンク２４，２５やアーム２６、ハンド２７，２８が折り畳まれた状態で待機している。そして、ハンド２７を用いてあるステージ６１内のワーク６０を取り出しに行くときは、まず、対象のステージ６１の開口の正面にハンド２７が位置するように、ＰＴＰ制御により図５において（ｂ）に示す進入位置にまでロボット１０を移動させる。その後、ＣＰ制御によって、ハンド２７がステージ６１の内部に進入するように、図５において（ｃ）に示すステージ位置にまでロボット１０を移動させる。ステージ位置においてワーク６０がハンド２７上に載置されたら、ロボット１０は進入位置を介して待機位置に戻り、次に、ワーク６０の搬送先となる別のステージ６１にまで上記と同様に移動する。ステージ位置は、ステージ６１内のワーク６０をハンド２７上に載置させる位置であり、ハンド２７上に既に載置されているワーク６０をステージ６１に対して積み下ろす位置でもある。これに対し進入位置は、そのステージ６１のステージ位置から作業領域に向けて引き戻した位置であって、ロボット１０が進入位置にあるときにロボット１０の一部またはロボット１０に載置されているワーク６０の一部がそのステージ６１の内部にある位置である。図示した例では、ロボット１０が進入位置にあるとき、ハンド２７の先端部がステージ６１の内部にある。

【0037】

図６（ａ）は、図４に示したロボット１０をＰＴＰ制御で移動させた例を示す概略斜視図であって、移動前の姿勢（姿勢Ｒ）と移動後の姿勢（姿勢Ｓ）とを示している。図６（ｂ）は、姿勢Ｒから姿勢ＳまでＰＴＰ制御でロボットを移動させたときの各軸の軸速度の変化を示している。ＰＴＰ制御において移動の始点において各軸が一斉に動き出しかつ移動の終点において各軸が同時に停止させるように制御するときは、異なる軸の間でのモータ速度の比率は、加減速の期間も含めて常に一定である。異なる軸の間でのモータ速度の比は、移動の始点から終点までの各軸のモータの移動量によって定まりこれは始点と終点の座標情報から算出できるから、ＰＴＰ制御の動作指令が入力された段階で計算できる。また、ＰＴＰ制御を行なうときは、そのＰＴＰ制御での移動の全体としての速度を示す全体速度が指令されることが一般的である。モータにはそれぞれ最高速度が規定されているが、全体速度１００％とは、ＰＴＰ制御により最速でロボットを移動できることを意味し、そのときは、いずれかの軸においてその軸のモータをそのモータに規定されている最高速度で動作させ、それ以外の軸では当該軸に規定されている最高速度未満でモータを動作させることになる。モータごとの規定されている最高速度と、ＰＴＰ動作時の異なる軸の間でのモータ速度の比率とが分かっているから、全体速度が１００％であるときに、各軸のモータがそのモータに規定されている最高速度に対して何パーセントの速度で動作しているかという割合（速度割合）を求めることができる。

【0038】

上述したように本実施形態では、モータごとに、そのモータの現在速度がそのモータに規定されている最高速度の何パーセントであるかに応じてそのモータに対する衝突検出リミット値を変化させることとしている。一方、ＰＴＰ制御を行なうときは、軸ごとに個別にモータ速度が指令されるのではなく全体速度が指令される。ティーチングを行うときなどは全体速度１００％でロボット１０を動作させるのはなく、より低速でロボット１０を動作させることとなる。例えば、全体速度のＸ％でロボット１０を動作させるときは、各軸のモータのそのモータの最高速度に対する速度比は、全体速度が１００％のときの速度割合に対してＸ／１００を乗じたものとなる。そして各軸の衝突検出リミット値として、このようにして得られたモータごとの速度比に応じたものを用いればよいことになる。ロボット制御装置５０では、ＰＴＰ制御に関して目標位置や全体速度の何％でロボットを動作させるかという情報は演算部５１に指令として与えられるので、演算部５１は各軸のモータの動作速度を算出でき、ロボット制御装置５０において軸ごとに設けられる閾値発生部５３には、当該軸について算出されたモータ速度が与えられる。このとき、表２に示すようにモータの速度範囲が複数の速度領域に分割されているときは、算出されたモータ速度がどの速度領域に含まれるかによって衝突検出リミット値が定まる。このようにして各軸に対してその軸の速度に応じて衝突検出リミット値を割り当てることで、より適切な衝突検出リミット値を用い衝突検出を行うことができるようになる。

【0039】

図７（ａ）は、ハンド２７の軌跡が直線となるように直線補間を行うＣＰ制御により図４に示したロボット１０を移動させた例を示す概略斜視図であって、移動前の姿勢（姿勢Ｒ）と移動後の姿勢（姿勢Ｓ）とを示している。図７（ｂ）は、姿勢Ｒから姿勢ＳまでＣＰ制御でロボットを移動させたときの各軸の軸速度の変化を示している。図７（ｂ）から明らかになるように、モータの軸速度の増減の様子は、モータごとに大きく異なっている。異なる軸のモータの速度の比は一定ではなく、移動の始点から終点までの間で大きく変化する。モータ速度の比やその変化は、移動の始点と終点の座標情報を与えただけで得られるものではなく、実際にＣＰ制御でロボット１０を動作させなければ分からない。したがって、ＰＴＰ制御の場合とは異なりＣＰ制御では、ロボット１０の全体の速度を指定を伴なってＣＰ制御の動作指令が入力されたとして、各軸ごとの衝突検出リミット値をその場で割り当てることができない。なお、ＣＰ制御では各軸のモータの速度変化が事前には分からないので、いずれかの軸のモータの速度がそのモータに規定された最高速度となり、残りの軸のモータの速度はそれらのモータにそれぞれ規定された最高速度未満である、という条件で全体速度における１００％を規定することは難しい。その代わり、ロボット制御装置５０では、マージンを見込んでＣＰ制御におけるロボット１０の全体の速度の最高値が定められており、その速度の最高値をＣＰ制御における全体速度の１００％とする。

【0040】

ところで、ＣＰ制御ではＰＴＰ制御に比べてロボットを素早く動かすことができないので、搬送用のロボットにおいては、ＣＰ制御を用いる場面は限定的である。図４に示した搬送用のロボット１０の場合であれば、図５を用いて説明したように、ワーク６０の搬送元あるいは搬送先となるステージ６１への進入または退出、すなわち進入位置とステージ位置との間の移動にだけＣＰ制御が使用され、それ以外の移動にはＰＴＰ制御が用いられる。ステージ６１の場所は固定されていることが一般的であり、ステージ６１の場所が固定されていれば、ステージ６１ごとにそのステージ６１のステージ位置に対するＣＰ制御でのロボット１０の移動軌跡は変化しないと考えられる。言い換えれば、ステージ６１に対するロボット１０のティーチング（教示）が完了した後は、ロボット１０は、そのステージ６１に対してＣＰ制御で移動するときに、毎回、同じ軌跡を通過する。

【0041】

そこで本実施形態では、ステージ６１ごとに衝突検出リミットテーブルを用意して記憶部５５に格納し、ＣＰ制御時には閾値発生部５３は衝突検出リミットテーブルを参照して得た閾値を比較部５４に出力する。図８は、ステージ＃１とステージ＃２の２つのステージ６１があるとして。衝突検出リミットテーブルの内容の一例を示している。図示されるようにステージ６１ごとに設けられる衝突検出リミットテーブルは、軸ごとにかつ全体速度での速度領域ごとに閾値である衝突検出リミット値を示したものである。全体速度での速度領域のことをＣＰ用（あるいは連続経路制御用）速度領域と呼ぶ。これに対し、表２を用いて説明したようにモータについて規定された最大速度に基づいて定められる速度領域のことをＰＴＰ用（あるいはポイント・ツー・ポイント用）速度領域と呼ぶ。表２に示す場合と同様に、テーブルにおいて全体速度の「１０％」は全体速度が０％以上１０％以下である場合に対応し、「２０％」は全体速度が２０％を超えて３０％以下である場合に対応する。例えば、ステージ＃１においてＣＰ制御の全体速度が４５％であるならば、Ａ軸に対応する閾値発生部５３は、ステージ＃１用の衝突検出リミットテーブルにおいて全体速度が「５０％」の欄を参照し、Ａ軸に対する衝突検出リミット値として２４０ｃＮ・ｍを取得して閾値として比較部５４に出力し、同様にＢ軸に対応する閾値発生部５３は、Ｂ軸に対する衝突検出リミット値として２００ｃＮ・ｍを取得する。他の軸についても同様である。

【0042】

ステージ６１ごとの衝突検出リミットテーブルは、ロボット１０のＣＰ制御でのそのステージ６１に対するティーチングが完了した時点で、そのステージ６１におけるＣＰ制御での各軸の速度割合を測定し、軸ごとの衝突検出リミット値を算出することによって作成することができる。具体的は、該当するステージ６１への移動動作に際して駆動部５２内での各軸のトルク補正値を測定してトルク波形を求め、トルク波形から軸ごとの最大トルク補正値を求め、軸ごとの最大トルク補正値にロボットごとの個体差を吸収するためのマージンを加算したものを軸ごとの衝突検出リミット値とする。ロボット制御装置５０に設けられているテーブル生成部５６は、駆動部５２からのトルク補償値の取得から軸ごとの衝突検出リミット値の算出までの処理を実行し、そのときの対象となっているステージとそのときの全体速度とに基づいて、算出された衝突検出リミット値を記憶部５５内の衝突検出リミットテーブルに書き込む。このような衝突検出リミット値の算出を、ロボット１０によってアクセスされるすべてのステージ６１と全ての速度領域（全体速度に関して衝突検出リミットテーブルにおいて用いている全ての速度領域）とについて実行することにより、ステージ６１ごとの衝突検出リミットテーブルが完成する。ひとたび衝突検出リミットテーブルが完成すれば、それ以降は、ロボット１０がＣＰ制御によりステージ６１にアクセスするときに、衝突検出リミットテーブルから読み出した衝突検出リミット値を各軸に割り当てることによって、ＣＰ制御においてロボット１０を動作させるときに、より最適な衝突検知を行うことができるようになる。

【0043】

以上説明した本実施形態の衝突検出方法によれば、モータのトルク値が衝突検出リミット値を超えたときに衝突発生と判定する場合において、モータ速度に応じて衝突検出リミット値を変化させるので、ロボットが低速で運動しているときであっても衝突の発生を速やかかつ確実に検出できる。特に、ステージごとかつ軸ごとに全体速度ごとの閾値をあらかじめ格納した衝突検出リミットテーブルを用意しておき、ＣＰ制御でロボットを実際に移動させるときには指令された全体速度によって衝突検出リミットテーブルを参照して閾値である衝突検出リミット値を得ることによって、ＣＰ制御によってロボットをステージにアクセスさせる際に、より最適な衝突検出を行うことができる。ロボットがＰＴＰ成語で動作しているときにおいても、衝突の発生を確実検出できる。

【符号の説明】

【0044】

１０…ロボット；１１，１１Ａ，１１Ｂ，１１Ｃ，１１Ｄ，１１Ｚ…モータ；１２…エンコーダ；２２…基台；２３…リンク機構；２６…アーム；２７…上側ハンド；２８…下側ハンド；２９…昇降部；５０…ロボット制御装置：５１…演算部；５２…駆動部；５３…閾値発生部；５４…比較部；５５…記憶部；５６…テーブル生成部。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】