JP2008188694A - ロボット安全監視装置及びロボット駆動制御システム - Google Patents

Abstract

【課題】不必要な非常停止の発生を抑制すると共に、ロボットの教示基準点の現実の移動軌跡が動作許可領域内に存在するか否かの判断をより確実に実施して安全性を向上させる。
【解決手段】ロボットの教示基準点の実空間上における指令座標が、ロボットの各軸を駆動するモータの動作変位量に関する指令値に基づいて算出される。指令座標が、動作許可領域内に存在するか否かが判定される。各軸の実際値から実空間における教示基準点の実際座標が演算され、現在時刻から過去所定数分の指令座標を時系列順に結ぶことによって得られる指令経路までの、現在の実際座標から計った距離に基づいて、実際値と指令値との関係が適正か否かが判定される。指令座標が前記動作許可領域内にないと判定された場合、又は、実際値と指令値との関係が適正でないと判定された場合の何れかの場合には、ロボットを安全な状態にするための非常停止信号が出力される。
【選択図】図８

Description

本発明は、ロボットの例えばツール先端にとられた教示基準点の移動経路が、他の物体などと干渉しない安全な所定の範囲内にあるか否かを監視するロボット安全監視装置及びその装置を用いたロボット駆動制御システムに関する。

ロボットの教示基準点の移動経路が他の物と干渉しない安全な所定の範囲内に存在するか否かを監視するためのロボット安全監視装置としては、例えば下記の特許文献１に記載されている産業用ロボット制御装置などが、従来より広く知られている。この装置では、ロボット制御装置自身が、現実のパルス分配前に、パルス分配後の位置を予測して、その位置が禁止座標領域に含まれのか否かを判定している。そして、予測位置が禁止座標領域に含まれると判定された場合には、その補間点位置にロボットの先端の教示基準点（ツール先端点）を現実に移動させる前に、ロボットの動作が停止される。しかし、この装置は、ロボットの移動量を指令するロボット制御装置自身が、移動後の位置を監視しているために、ロボット制御装置自体が、故障したり暴走したりする場合には、教示基準点が禁止座標領域内に存在するか否かの判定が実行できず、教示基準点が禁止座標領域内に移行することを未然に防止することができない。

そこで、この問題を解消するために、下記特許文献２では、ロボットの動作を指令するロボット制御装置とは、別体の安全コントローラを設けて、この安全コントローラにより、ロボットの動作経路を監視するようにしている。すなわち、特許文献２では、ロボットの各軸の実際の角度をレゾルバで検出して、教示基準点の直交座標系における現実の位置を求め、この現実の位置が、設定されている動作許可範囲に属するか否かを判定してる。そして、各軸の測定値から得られる現実の位置が動作許可範囲に存在しない場合に、ロボットを非常停止させるようにしている。また、各軸の実際の角度範囲が、予め設定されている各軸の動作許可範囲内に存在するか否かを判定して、動作許可範囲に存在しない場合に、ロボットを安全位置に非常停止させるようにしている。
特許第２６６２８６４号公報 特表２００１−５２５２６３号公報

しかしながら、上記特許文献２の技術によると、ロボットの各軸のレゾルバにより検出された現実の角度により、ロボットの教示基準点の床に固定された直交座標系における位置を演算し、この位置が設定された動作許可範囲に存在しない場合に、安全位置へ教示基準点を移動させる非常停止が実行されている。ロボットにおいては、環境の温度変化によるロボットアーム長の変動、負荷変動によるロボットアームの撓み変動、回転軸の遊びの変動、加速力、減速力によるロボットアームの撓み変動等が存在する。このために、各軸の検出角度に誤差がなくとも、検出角度から得られる教示基準点の直交座系における予測位置と、その教示基準点の真の現実の位置とは異なったものとなる。すなわち、教示基準点の真の現実の位置は動作許可範囲内にあっても、各軸の回転角から演算された教示基準点の直交座標系における座標は、動作許可範囲外に存在し、ロボットを非常停止させる場合がある。逆に、教示基準点の真の現実の位置は動作許可範囲外にあっても、各軸の回転角から演算された教示基準点の直交座標系における座標は、動作許可範囲内に存在し、ロボットを非常停止させない場合も発生し得る。

また、ロボットの教示時点においては、教示基準点の理論上の動作軌跡に基づいて、動作許可範囲が設定される。この場合に、教示後の初期の運行においては、正常な監視動作が達成される。しかし、運行期間が長くなると、環境の温度変化によるロボットアーム長の変動、回転軸の遊びの変動などにより、教示基準点の真の現実の位置は理論上の動作軌跡に対して乖離したものとなり、漸次、動作許可範囲内の動作軌跡であってもその余裕が小さくなる。この結果、負荷の増大によるロボットアームの撓み量の増加、加速力、減速力の増加によるロボットアームの撓み量の増加などを原因として、教示基準点の理論上の動作軌跡や真の現実の動作軌跡は、動作可能範囲内に存在しても、各軸の検出された回転角から教示基準点の直交座標系における座標に変換した値による動作軌跡の一部が、突然に、動作可能範囲外になる場合もある。この場合には、不必要に非常停止を発生させることになる。特に、動作許可範囲を、理論上の教示基準点の移動軌跡に対する余裕を小さくして設定した場合には、長い期間、この監視による非常停止が発生しなくとも、僅かな負荷変動や加速度や減速度の増加により、作業中に、非常停止が頻繁に発生することが起こり得る。

本発明は、上記の課題を解決するために成されたものであり、その目的は、真の現実の移動軌跡が動作許可領域外に存在しないのに、不必要に非常停止が発生することを減少させることである。また、他の目的は、不必要な非常停止の発生を抑制すると共に、ロボットの教示基準点の真の現実の移動軌跡が動作許可領域内に存在するか否かの判断をより確実に実施し、これにより、より安全性と運用効率とを向上させることを目的とする。

上記の課題を解決するためには、以下の手段が有効である。
すなわち、第１の発明は、ロボットの教示基準点の移動経路が安全な所定の範囲内にあるか否かを監視するロボット安全監視装置において、ロボットを制御するロボット制御装置から出力された、ロボットの各軸を駆動するモータの動作変位量に関する指令値を入力する指令値入力手段と、動作変位量の変化に伴って運動するロボットの教示基準点の実空間上における指令座標を指令値に基づいて算出する指令座標算出手段と、実空間上において教示基準点が位置することが許される所定の動作許可領域を記憶する動作許可領域記憶手段と、指令座標算出手段によって算出された指令座標が、動作許可領域記憶手段に記憶されている動作許可領域内に存在するか否かを判定する第１の判定手段と、第１の判定手段によって指令座標が動作許可領域内に存在しないと判定された場合に、ロボットを安全な状態にするための非常停止信号を出力する非常停止信号出力手段とを有することを特徴とするロボット安全監視装置である。

ここで、ロボットの教示基準点とは、動作経路を特定するためのロボットの移動部材にとられた基準点である。例えば、フランジ中心点、ツール中心点、ツール先端点などであり、ロボットにさせるべき作業内容によって、この教示基準点の位置は変動し得る。また、教示基準点は、プログラムやマニュアルでロボットの位置や姿勢を教示する場合の実空間における位置を指定するための点である。本発明は、教示した理論上の指令座標による動作軌跡が動作許可領域外になれば、非常停止がかかる。上記のロボットの軸（自由度）の数は任意でよい。また、上記のモータは、必ずしもロータを有するものでなくともよく、例えばリニアモータなどであってもよい。

第２の発明は、第１の発明において、さらに、ロボットの各軸を駆動するモータの動作変位量に関する実際値を入力する実際値入力手段と、実際値と指令値との関係が適正であるか否かを判定する第２の判定手段とを有し、非常停止信号出力手段は、第１の判定手段によって指令座標が動作許可領域内に存在しないと判定された場合、又は、第２の判定手段によって実際値と指令値との関係が適正でないと判定された場合の何れかの場合には、ロボットを安全な状態にするための非常停止信号を出力するようにしたことを特徴とする。

この第２の発明では、教示した理論上の指令座標による動作軌跡が動作許可領域に存在し、且つ、実際値と指令値との関係が適正である場合にのみ、ロボット制御装置によるロボットの動作が可能となり、その他の場合には、非常停止がかかる。すなわち、仮に、指令座標による動作軌跡が動作許可範囲内に存在していても、実際値と指令値との関係が適正でない場合には、非常停止がかかることになる。本第２の発明は、このように、教示基準点が動作許可範囲に存在するか否かを理論上の指令座標で判断し、その判断を補強するために実際値と指令値との関係が適正か否かを判断するものである。すなわち、まずは、論理上の動作軌跡が動作許可範囲内に存在するか否かを判断し、その理論上の動作軌跡の正当性を評価するのに、その動作軌跡に対する現実の動作軌跡の乖離の程度を用いている。
したがって、指令座標が動作許可範囲外になれば、当然に、ロボットは非常停止するが、指令座標が動作許可範囲内であっても、理論上の動作軌跡に対する現実の動作軌跡の乖離が所定値よりも大きくなると予測される場合には、ロボットを非常停止させるようにしたことが本発明の特徴である。

上記の第２の判定手段においては、実際値と指令値との関係が適正であるか否かを判定する。指令値と実際値とを各軸の座標（角度）で比較しても、指令値と実際値との各軸の座標を、教示基準点の実空間における座標に変換して、実空間における座標で比較しても良い。実空間は、直交座標系、円筒座標系、極座標径など座標系は任意である。さらに、他の仮想の座標空間に変換した座標で比較しても良い。また、指令値と実際値との関係が適正か否かは、指令値と実際値との偏差、その偏差に関係する値が所定範囲に存在しているか否かにより判断される。

また、第３の発明は、第２の発明において、教示基準点の実空間上における実際座標を実際値に基づいて算出する実際座標算出手段を設け、第２の判定手段は、指令座標と実際座標との偏差に基づいて、実際値と指令値との関係が適正か否かを判定することである。 ただし、指令座標や実際座標は、必ずしも直角座標である必要はなく、極座標や円筒座標などの、実空間上に張られた任意の座標を用いることができる。指令座標と実際座標との偏差が所定範囲内に存在する場合には、実際値と指令値との関係が適正であると判断し、そうでない場合には不適正であると判断する。偏差は、一般的には、指令座標と実際座標との間の距離である。しかし、各座標軸毎の偏差を求めて、何れか一つの偏差の大きさに基づいて実際値と指令値との関係が適正か否かを判定するようにしても良い。

また、第４の発明は、第２の発明において、教示基準点の実空間上における実際座標を実際値に基づいて算出する実際座標算出手段を設け、第２の判定手段は、実際座標に対応する時刻から過去所定数分の指令座標から得られる指令経路と、現在の前記実際座標との間の距離に基づいて、実際値と指令値との関係が適正か否かを判定する。
ただし、実際座標は、必ずしも直角座標である必要はなく、極座標や円筒座標などの任意の座標を用いることができる。また、指令経路には、指令座標を時系列順に結ぶことによって得られる折れ線状の経路を用いることができる。その他、指令経路には、例えば、複数の指令座標を通過する多項式などの関数、複数の指令座標を最小自乗近似する多項式などの関数による曲線などで決定しても良い。すなわち、本発明により、教示基準点が描く現実の動作軌跡の、理論上の動作軌跡に対する乖離の程度が測定されることになる。

また、第５の発明は、第２の発明において、第２の判定手段は、指令値の単位時間当たりの変位から得られる各軸の指令速度と所定遅延時間との積を閾値として、実際値と指令値との関係が適正か否かを判定することを特徴とする。
すなわち、各軸の実際値は、サーボ制御による遅延により、各軸の指令値に対して指令値の速度に応じて遅れる。この遅延量は、誤差にはならないので、この遅延量を考慮して、指令値と実際値との関係が適正か否かを判断するようにしても良い。通常は、教示基準点の理論上の動作軌跡に対して、サーボ制御による遅延量を考慮した上で、動作許可領域が設定されているので、実際値と指令値との間にこの遅延量だけの偏差があっても問題にはならない。したがって、各軸の実際値（角度）と、その各軸の指令値との偏差からこのサーボ遅延量を減算した値と、閾値とを比較することで、実際値と指令値との関係が適正か否かを判断することができる。逆にいえば、偏差の大きさを判定する閾値を遅延量だけ補正すれば良い。

この判定は、実空間における教示基準点に関して実行しても良いので、実空間で行うようにしたのが、第６の発明である。すなわち、第６の発明は、第３又は第４の発明において、第２の判定手段は、指令座標の単位時間当たりの変位から得られる指令座標の指令速度と所定遅延時間との積を閾値として、実際値と指令値との関係が適正か否かを判定することを特徴とする。
実空間上において、教示基準点の実際座標は、教示基準点の指令座標に対して、指令座標の速度に応じて遅れる。この遅延量は、誤差にはならないので、この遅延量を考慮して、指令値と実際値との不一致を判断する。すなわち、指令座標と実際座標との偏差から、遅延量を減算した値を閾値と比較することで、実際値と指令値との関係が適正か否かを判断することができる。逆に言えば、この偏差の大きさを判定する閾値を遅延量だけ補正すれば良い。

更に、第７の発明は、第２乃至第６の何れかの発明において、指令座標算出手段によって算出された前回の指令座標と今回の指令座標との差分に基づいて得られる指令速度が、所定の適正速度範囲内に存在するか否かを判定する第３の判定手段を有し、非常停止信号出力手段は、第１の判定手段によって指令座標が動作許可領域内に存在しないと判定された場合、第２の判定手段によって実際値と指令値との関係が適正でないと判定された場合、又は、第３の判定手段によって指令速度が適正速度範囲内に存在しないと判定された場合の何れか一つの場合には、ロボットを安全な状態にするための非常停止信号を出力するようにしたことである。

また、第８の発明は、産業用ロボットを駆動制御するロボット駆動制御システムにおいて、第１乃至第７の何れかの一つの発明にかかるロボット安全監視装置を有することを特徴とするロボット駆動制御システムである。

以上の本発明の手段によって得られる効果は以下の通りである。
すなわち、第１の発明によれば、まず、上記の第１の判定手段により、教示基準点の実空間における指令座標が動作許可領域に存在しないと判定された場合には、非常停止信号が出力される。これにより、ロボットの経路を初めて教示したり経路のプログラミングをして動作許可領域を設定した後に、試し運転をするすることで、非常停止信号の出力の有無の正当性を評価することができる。そして、ロボットの負荷を変更したり、加速度、減速度を変更したりして、試し運転を実行しつつ、最小限の適正な動作許可領域を設定することができる。このようにして、ロボットの動作状態による変動や径年変化に基づく変動量を考慮した適正な動作許可領域が設定された後は、教示基準点の理論上の動作軌跡が動作許可領域内に存在することが確保されている。したがって、動作環境温度の変動、負荷変動、加速度や減速度の変化、機械の径年変化が仮にあっても、その後の実際の運転に際して、教示基準点の現実の動作軌跡が動作許可領域に存在するにも係わらず非常停止信号が出力されたり、現実の動作軌跡が動作許可領域に存在しないにも係わらず非常停止信号が出力されないという可能性は減少する。また、指令値がノイズの影響で変化することにより、指令座標が動作許可領域内に存在しなくなる場合には、非常停止信号が出力される結果、ノイズによる予期しないロボットの動作を防止することができる。

さらに、第２の発明では、現実の動作軌跡が動作許可領域に存在するにも係わらず非常停止信号が出力されたり、現実の動作軌跡が動作許可領域に存在しないにも係わらず非常停止信号が出力されないという場合を減少させるために、第２の判定手段を設けている。第２の判定手段によって、実際値と指令値との関係が適正か否かが判定される。すなわち、教示基準点の実際値と指令値との偏差の大きさが判定される。この偏差が小さいということは、教示基準点の現実の移動軌跡と理論上の移動軌跡との偏差も小さいことの可能性が高いことを意味する。したがって、２重の監視により、不必要な時には、非常停止信号を出力することなく、真に必要な時には、非常停止信号が必ず出力されるという状態を高精度で実現できる。また、指令値、実際値の双方の信頼性が確保される。すなわち、指令値や実際値は、外部からのノイズの影響を受け得るが、この場合に、両者の差が所定値以上に存在する場合に、非常停止が指令されるので、ノイズなどによる不確実性を排除することができる。

また、第３の発明によれば、実空間上における実際座標と指令座標との偏差に基づいて、第２の判定手段における適否判定を行うことができる。このため、本第３の発明によれば、実空間上における偏差を基準に判断しているので、非常停止信号の出力の有無の許容誤差範囲を直視的に理解することができる。

また、第４の発明によれば、現在の実際座標と、現時刻に対して所定期間過去の指令経路間の距離に基づいて、実際値と指令値との関係が適正か否かが判定される。したがって、教示基準点の理論上の動作軌跡に対する現実の動作軌跡の乖離の程度を正確に判定することができ、非常停止信号を出力すべきか否かの判定を正確に行うことができる。また、距離を判定の基準として設定するので、非常停止信号の出力の有無の許容誤差範囲を直視的に理解することができる。さらに、現実の教示基準点は、理論上の指令経路に対して、ある誤差を有して且つ一定の遅れをもって、追従する。したがって、ある時刻における教示基準点の指令座標と実際座標との偏差には、単なる追従遅れ成分も含まれている。しかし、指令経路と教示基準点の実際座標との距離（最小距離）には、この追従遅れ成分は含まれていない。よって、この距離を指令座標と実際座標との真の誤差として評価すれば、非常停止信号の出力の有無を判断するに必要な指令値と実際値との関係が適正か否かをより正確に判断することができる。

第５の発明によれば、サーボ制御による遅延量が補正されて、指令値と実際値との関係が適正か否か（一致か不一致か）が判断されるので、両者の一致不一致をより正確に判断することができる。また、第６の発明によれば、同様に、教示基準点の指令座標と実際座標の一致不一致を判断するのに、教示基準点の実空間におけるサーボ制御による遅延量が補正されている。したがって、指令値と実際値との間の一致不一致をより正確に判定することができる。これらの特徴により、現実には、教示基準点が動作許可領域内に存在するにも係わらず、不必要に非常停止信号が出力されることや、逆に、教示基準点が動作許可領域外に存在するにも係わらず、非常停止信号が出力されないとういうことが、精度良く防止される。この結果、より安全に寄与するロボット制御が可能となる。

第７の発明によれば、指令値に基づいて算定された指令速度が適正速度範囲内に存在するか否かが第３の判定手段により判定されるので、ロボットの動作速度に関する適合性を判定し、より確実な安全性を実現することができる。また、ロボットの教示基準点の指令値の時間特性が急変したような場合には、ロボットの教示基準点の指令座標に関する指令速度もそれによって非常に大きくなる。このような場合は、通常の正常指令では考えられないので、ノイズなどにより指令値が変化したと考えられる。このような場合にも第３の判定手段による判定により、非常停止信号が出力される。したがって、その様な場合にも、この第３の判定手段は、第２の判定手段における正常異常の判定に対する冗長な重複処理にもなり得るので、この様な判定処理の多重化によってより高度な安全性が確保できる。

また、本発明の第８の手段によれば、以上の作用・効果に基づいて、従来よりも安全性の高いロボット駆動制御システムを有する所望の安全な生産ラインを構成することができる。

以下、本発明を具体的な実施例に基づいて説明する。
ただし、本発明の実施形態は、以下に示す個々の実施例に限定されるものではない。

図１に本実施例１のロボット駆動制御システム１の主要構成を示す。このロボット駆動制御システム１は、主として、ロボット安全ユニット（Robot Safty Unit、ＲＳＵ、安全ＰＬＣとも言う）２３０、ロボット制御装置(Robot Controller 、ＲＣ）２２０、ＰＬＣ(Programmable Logic Controller) ２１０、安全監視装置(Robot Safety Controller、ＲＳＣ）１００、サーボユニット(Servo Unit)３００とで構成されている。ロボット制御装置２２０とロボット安全ユニット２３０との間、安全ユニット２３０と安全監視装置１００との間は、配線４に接続されている。また、ＰＬＣ２１０とロボット制御装置２２０とは内部バスにより相互に接続されている。ロボット制御装置２２０と安全監視装置１００とは通信線路３、通信線路５で接続され、ロボット制御装置２２０とサーボユニット３００とは通信線路８で接続され、サーボユニット３００と安全監視装置１００とは通信線路２で接続されている。そして、サーボユニット３００には、６軸ロボットの各軸モータＭが配線９により接続されている。このロボット駆動制御システム１は、６軸ロボットに対する制御システムである。図１は、６軸のうち、３つのモータＭを代表的に表示している。本実施例では６軸ロボットを示すが、５軸ロボット、その他の軸数のロボットを用いても良い。

このシステムでは、給電ライン７とサーボユニット３００とを介して、６つのモータＭに必要な電力を所定の電源から供給することができる。この給電ライン７への通電／遮電状態は、それぞれ直列に２重化されたマグネットスイッチＭＳによって確実に切り換えることができる。ロボット安全ユニット２３０は、配線６を介して、このマグネットスイッチＭＳのリレーコイルへの通電と遮電を制御して、マグネットスイッチＭＳのオンオフを制御する。ロボット安全ユニット２３０の制御処理に基づいて、このリレーコイルに継続的に給電されると、マグネットスイッチＭＳは導通状態に保持される。ロボット安全ユニット２３０には、作業者による非常停止を指令するための非常停止ボタンＥＳＢ２３４、作業者によりシステムの各装置に通電し、システムを運転可能状態とするマスターＯＮボタン２３２が設けられている。ロボット安全ユニット２３０は、リレー回路、又は、ＰＬＣ(Programable Logic Controller)で構成され、モータＭへの給電を制御するマグネットスイッチＭＳを制御するものであり、主として、非常停止を行う役割を担う。なお、システムの信頼性を向上させて高い安全性を確保するために、配線６は２重化されており、各配線６を伝搬する２重化された電流信号により、各マグネットスイッチＭＳを制御している。また、配線４も２重化されている。

図示しないロボットの動作は、ＰＬＣ２１０と、このＰＬＣ２１０に内部バスにより連結されたロボット制御装置２２０により制御される。ＰＬＣ２１０には、作業者によりロボットの動作の開始を指令するロボット・スタート・ボタン１２１が設けらている。ＰＬＣ２１０は、所定のラダーダイアグラムに相当するプログラムを実行することによって、ロボット駆動システム１の全体をシーケンス制御する。ロボット制御装置２２０は、ロボットの位置決め制御を実行するためのものであり、サーボユニット３００に対して、モータＭの各軸の回転角（動作変位量）に関する指令値βを逐次出力する。エンコーダＥは、モータＭの回転軸に直結しており、モータＭの各軸の回転角（動作変位量）に関する実際値αを、逐次、検出して、通信配線２を介して、安全監視装置１００に出力する。サーボユニット３００は、プログラムされた所定のＰＩ，ＰＩＤなどのフィードバック制御により、実際値αと指令値βとの偏差を減少させるように、各モータＭの回転角（動作変位量）をそれぞれ駆動制御する。実際値αは、６つのモータＭの実際の各回転角α１，α２，…，α６の組からなるベクトルとする。同様に指令値βも、指令の各回転角β１，β２，…，β６の組からなるベクトルである。ただし、これらの回転角の単位は任意でよい。

安全監視装置１００は、マイクロプロセッサを中心に構成されており、入力インターフェイス１０１（実際値入力手段）を有している。入力インターフェイス１０１は、通信線路２を介して、エンコーダＥから出力される実際値αを安全監視装置１００に入力するためのものである。また、安全監視装置１００の入力インターフェイス１０２（指令値入力手段）は、通信線路３を介して、ロボット制御装置２２０から出力される指令値βを安全監視装置１００に入力するためのものである。また、安全監視装置１００の有するメモリ１０４には、安全監視装置１００を動作させるためのプログラムを記憶したプログラム領域と、動作許可領域を定義するデータを記憶した動作許可領域データ記憶領域と、その他のデータを記憶するデータ領域とが、形成されている。配線４は、ロボット制御装置２２０とロボット安全ユニット２３０とを、安全ユニット２３０と安全監視装置１００との間を、それぞれ、接続している。信頼性を向上させて高い安全性を確保するために、これらの装置の内部回路は、２重化され、配線４も２重化されている。すなわち、配線４を伝送する信号が２重化され、各ユニットの処理も２重化されて、処理に冗長性が持たている。安全監視装置１００が有する２重化された出力インターフェイス１０３（非常停止信号出力手段）は、この２重化された配線４に接続されている。また、通信線路５は、ロボット制御装置２２０と安全監視装置１００とを接続している。通信線路５では、信号はイーサネットプロトコルに従って伝送される（イーサネットは登録商標）。

図２に本ロボット駆動制御システム１の運転の準備処理手順を示す。図２の準備処理手順は、安全性を確保して、本ロボット駆動制御システム１を動作可能なレディー状態に移行させるために、システムの再起動の際に予め実施しておくべき最初の前処理手順を示す。

ステップＳ１にて、作業者により、システム全体に給電するための電源スイッチが投入される。ただし、この時、マグネットスイッチＭＳは、勿論、オフ状態、すなわち、給電を遮断する状態のままであり、各軸は機械的制動が掛かった状態が維持される。
次に、ステップＳ２では、上記の各装置（１００，２１０，２２０，２３０，３００）が、それぞれ各装置単位で初期化される。これらの初期化処理は、これらの各装置自身によって各装置毎に実行される。

次に、ステップＳ３では、安全監視装置１００によって、ロボットの同一性が検査される。すなわち、前回のシステム停止時から今回のシステム立ち上げ時までの期間中に、駆動対象となるロボットに変更が有ったか否かについて検査される。この検査手続きは、ロボット制御装置２２０から通信線路５を介して安全監視装置１００へ転送されてくるロボットの型式パラメータと、メモリ１０４に記憶されているロボットの型式パラメータとが一致するか否かの判定処理によって実施される。そして、ロボットの変更が有った場合には、以下の準備手順の実行は中止される。

次に、ステップＳ４では、安全監視装置１００によって、通信線路２と通信線路３の各状態をそれぞれチェック及び初期化して、それらの回線状態を正常状態に確立する。これによって、実際値αや指令値βが、通信線路２や通信線路３を介して確実に通信できることが確認される。
次に、ステップＳ５では、安全監視装置１００によって、実際値αの同一性について検査される。前回のシステム停止時から今回のシステム立ち上げ時までの期間中に実際値αに変化が有ったか否かによって、これは検査される。すなわち、ロボットの実際の位置姿勢と、管理上の位置姿勢とが一致しているか否かが判定される。一致していない場合には、前回にロボットが停止された状態から、ロボットの位置姿勢に移動があったことを意味し、管理上の位置姿勢が真の値ではないので、ロボットを制御することができない。したがって、実際値αに変化が有った場合には、以下の準備手順の実行は中止される。

次に、ステップＳ６では、それまでに、安全監視装置１００から、出力インターフェイス１０３（非常停止信号出力手段）と配線４とを介して、ロボット安全ユニット２３０に対して、出力されている非常停止信号の出力が停止される。これにより、安全監視装置１００による非常停止指令は解除される。

最後に、ステップＳ７では、ロボット安全ユニット２３０のマスターＯＮボタンＭＯＢ２３２が作業者によって押下されたか否かが検出される。そして、ＭＯＢ２３２の押下が検出されると、ロボット安全ユニット２３０により、安全監視装置１００からの非常停止指令解除と、非常停止ボタンＥＳＢ２３４の解除と、その他の装置からの非常停止指令解除とが、全て、成立しているか否かが判定される。そして、これらの条件が全て成立している場合には、ロボット安全ユニット２３０により、マグネットスイッチＭＳは、ＯＮ状態とされる。そして、ロボットの各軸は、機械的保持が解除されて、モータＭへの通電により静止姿勢を保持するサーボロック状態となる。これによって、以後、システムは、ロボットがいつでも動作を開始できるレディー状態となる。

図３に、ロボット・スタート・ボタンＲＳＢ２１２を押すことによって開始されるその後の通常動作時に、上記の安全監視装置１００によって実行される監視処理手順（安全監視処理１）を示す。
まず最初に、ステップＳ１１（実際値入力手段）において、入力インターフェイス１０１（実際値入力手段）を介して、各エンコーダＥから出力される各軸の実際値α＝（α１，α２，…，α６）が読み取られる。次に、ステップＳ１２（指令値入力手段）では、ロボット制御装置２２０からリアルタイムでサーボユニット３００に出力される、各補間点に教示基準点を移動させるための各軸の指令値β＝（β１，β２，…，β６）が入力インターフェイス１０２（指令値入力手段）を介して、読み取られる。

次に、ステップＳ１３（実際座標算出手段）では、ステップＳ１１で得られた実際値αに基づいて、位置制御の対象であるロボットの教示基準点の実際座標Ａが演算される。ロボットが設置されいる床に固定されたｘｙｚ直交座標系でこの座標が定義される場合には、各成分（Ａ_x ，Ａ_y ，Ａ_z ）を求めればよい。すなわち、このステップＳ１３は、第３の発明に係る実際座標算出手段の一例に相当するものである。各軸の回転角は、例えばツール先端点である教示基準点のｘｙｚ直交座標系の値に変換される。この変換手法は、ロボットアームの長さなどロボットの幾何学的パラメータを用いた変換式を用いるものであり、公知である。一般的には、教示基準点の位置と姿勢を表すものとして、４×４の行例が用いられる。その行列成分のうちの位置を表す成分（Ａ_x ，Ａ_y ，Ａ_z ）が実際座標として求めるものとなる。

次に、ステップＳ１４（指令座標算出手段）では、ステップＳ１２で得た指令値βに基づいて、位置制御の対象であるロボットの教示基準点の指令座標Ｂを求める。ｘｙｚ直交座標系でこの座標を定義する場合には、各成分（Ｂ_x ，Ｂ_y ，Ｂ_z ）を求めればよい。この指令値は、各軸の移動量で与えているので、上記の実際座標と同様に、各軸の回転角から教示基準点の指令座標が求められる。

次に、ステップＳ１５（第２の判定手段）では、ステップＳ１３、Ｓ１４で得られた実際座標Ａと指令座標Ｂとが比較される。すなわち、このステップＳ１５は、第３の発明の第２の判定手段の一例に相当するものである。ただし、この比較処理は、ｘ，ｙ，ｚの各座標成分毎に実施することができ、ｘ軸成分に関して言えば、例えば以下の条件式が満たされるか否かを判定することによって行うことができる。
（指令座標Ｂが満たすべき条件式の例１）
Ａ_x −δ_x1≦Ｂ_x ≦Ａ_x ＋δ_x2 （δ_x1≧０，δ_x2≧０） …（１）
ただし、ここで、閾値δ_x1，δ_x2は、適当な定数であってもよいし、指令速度または実際速度などに依存する変数などであってもよい。指令座標Ｂの適正指令範囲を規定するこれらの各値δ_x1，δ_x2などは、ロボットやモータの特性、負荷重量、サーボ遅れなどを加味して決定することがより望ましい。

また、上記の条件式（１）の代わりに、次の様な差分ベクトル（Ｂ−Ａ）を用いたより簡潔な条件式（２）を用いてもよい。なお、下記の閾値Ｄについても、ロボットやモータの特性、負荷量、サーボ遅れなどを加味して決定することが望ましい。すなわち、指令座標Ｂと実際座標Ａとの距離が閾値Ｄ以下である場合には、両者の関係が適正であると判断される。
（指令座標Ｂが満たすべき条件式の例２）
｜Ｂ−Ａ｜≦Ｄ （Ｄ＞０） …（２）
そして、指令座標Ｂが実際座標Ａを基準とする所定の適正指令範囲内にあれば、指令値と実際値との関係は適正であると判断されてステップＳ１６へ、そうでなければ、指令値と実際値との関係は適正でないと判断されてステップＳ１９（非常停止信号出力手段）へ移行し、非常停止信号が出力される。

次に、ステップＳ１６（第１の判定手段）では、ロボットが配置されている床に固定された実空間の直交座標系において上記の教示基準点が位置することが許される所定の動作許可領域Ｚに指令座標Ｂが含まれているか否かが判定される。この動作許可領域Ｚは、予め図１のメモリ１０４の動作許可領域データ記憶領域（動作許可領域記憶手段）によって記憶されている実空間上の固定領域である。図４に示すように、動作許可領域Ｚを、連続した多数の直方体（大きさは任意で各面は各座標面に平行で、各直方体は一部重なっていても良い）の集合体で近似する。図４では、連続した３つの直方体の集合で動作許可領域Ｚを近似した例が示されている。この各直方体の一つの角を基準点Ｐ_k とし、各直方体Ｗ_k 毎に、この基準点Ｐ_k の座標（Ｘ_k0, Ｙ_k0 Ｚ_k0）と３辺の長さ（Ｘ_k , Ｙ_k , Ｚ_k ）が、図５に示すようなテーブルとして、メモリ１０４の動作許可領域データ記憶領域に記憶されている。

そして、指令座標Ｂが、動作許可領域Ｚに存在するか否かの判定は、その領域Ｚを構成する各直方体Ｗ_k の内部に指令座標Ｂが存在するか否かを判定することにより実行される。例えば、直方体Ｗ_k に、指令座標が存在するか否かは、Ｘ_k0≦Ｂ_x ≦Ｘ_k0＋Ｘ_k ，Ｙ_k0≦Ｂ_y ≦Ｙ_k0＋Ｙ_k ，Ｚ_k0≦Ｂ_z ≦Ｚ_k0＋Ｚ_k の不等式の成否により決定される。これを全ての直方体成分について実行することで、いずれの直方体成分にも指令座標Ｂが属さない場合には、指令座標Ｂは、動作許可領域Ｚに属しないと判断される。なお、動作許可領域Ｚは教示基準点の実際の軌跡に対して一定の余裕を見て設定しても良いし、上記の判定の式において、所定の余裕を加味しても良い。このような処理が、教示基準点が、順次連続的に各補間点に位置決め制御される毎に実行される。そして、指令座標Ｂが動作許可領域Ｚ内にあれば、ステップＳ１７へ、そうでなければステップＳ１９へ移行して、非常停止信号が出力される。

次に、ステップＳ１７（第３の判定手段）では、次式（３）に基づいて、教示基準点に関する指令速度Ｖが求められる。
（指令速度Ｖ）
Ｖ＝（Ｂ（ｎ）−Ｂ（ｎ−１））／Δｔ …（３） ただし、ここで、指令速度Ｖはベクトルであり、Ｂ（ｎ）は今回求めた最新の指令座標であり、Ｂ（ｎ−１）は前回求めた指令座標であり、Δｔはその間の時間である。

次に、ステップＳ１８では、指令速度Ｖの絶対値｜Ｖ｜と指令速度に関する所定の上限値Ｖ_MAX とを比較し、｜Ｖ｜の値がＶ_MAX 以下であれば処理をステップＳ１１に戻し、そうでなければステップＳ１９に移行して、非常停止信号が出力される。

ステップＳ１９（非常停止信号出力手段）では、安全監視装置１００の出力インターフェイス１０３（非常停止信号出力手段）から、ロボット安全ユニット２３０、ロボット制御装置２２０、ＰＬＣ２１０に対して、非常停止信号（ＲＳＣ−ＥＭＳ）が出力さる。指令座標Ｂと実際座標Ａとの偏差が所定範囲を越える場合、指令座標Ｂが動作許可領域Ｚに属しない場合、指令速度が所定値を越える場合、何れか１つの状態が成立すると、非常停止信号が出力されて、ロボット安全ユニット２３０により、マグネットスイッチＭＳがオフ状態にされ、サーボユニット３００、モータＭへの給電が停止される。また、ロボットの各軸は機械的制動が掛けられて、緊急停止時の姿勢が機械的に保持されることになる。

以上の様な構成に従えば、指令座標Ｂに基づいて、まず、動作許可領域Ｚ内に教示基準点が存在するか否かが判定されるために、ロボットの負荷変動や、温度変動、各軸の回転余裕の径年変化があっても、不必要に、非常停止が発生することがない。また、指令座標と実座標との偏差が所定値よりも大きくなった場合にも、非常停止となるため、現実の教示基準点が理論上の動作軌跡から所定値以上に外れるような動作は禁止され、安全運転が確実に実現される。また、指令速度が所定値を越える場合にも、非常停止することから、より安全性が確保されることになる。また、外部ノイズによって、指令座標Ｂが急変したり、実際座標Ａが急変したような場合にも、確実に、非常停止を実現することができる。

なお、指令速度と実際速度の偏差が閾値を越える場合に、非常停止信号を出力するようにしても良い。また、上記の速度による判定を除外して、指令座標Ｂが動作許可領域Ｚに存在すか否かの判定と、指令座標と実際座標との偏差が所定範囲に存在するか否かの判定だけであっても良い。また、動作許可領域Ｚを連続した直方体の集合で近似したが、これを、図６に示すように、連続した多数の球体（大きさは任意、球体の一部は他の球体の一部と重なっていても良い）の集合体で近似しても良い。この場合には、各球体の中心の直交座標と半径とが、図７に示すようなテーブル形式で、メモリ１０４の動作許可領域データ領域に記憶されることになる。

図８に、上記の安全監視装置１００の本実施例２におけるその他の監視処理手順（安全監視処理２）を例示する。この安全監視処理２は、実施例１の安全監視処理１（図３）のステップＳ１５，Ｓ１６をステップ群Ｇ１５０に置き替えたものであり、その他の各ステップについては、実施例１の安全監視処理１と同様に動作する。
このステップ群Ｇ１５０のステップＳ１５１，Ｓ１５２，Ｓ１５３は、図３のステップＳ１５に代わる判定処理を行うためのものであり、第４の発明における第２の判定手段の１例に相当する。

即ち、ステップＳ１５１では、経路上の補間点の過去一定期間の指令座標の集合から、図９に例示する様な指令経路Γを求める。この指令経路Γは、既に算出した例えば４つの指令座標Ｂ（ｎ），Ｂ（ｎ−１），Ｂ（ｎ−２），Ｂ（ｎ−３）を時系列順に結ぶことによって得られる。そして、次のステップＳ１５２では、今回求めた最新の実際座標Ａ（ｎ）からこの指令経路Γに下ろした垂線の長さＬを求める。この長さは、隣接する２つの指令座標点を通る直線の方程式を求め、その方程式と実際座標Ａ（ｎ）から、その直線に対する距離を演算すれば良い。各直線と実際座標Ａ（ｎ）との各距離を求め、そのうちの最小値を、求める長さＬとすれば良い。この長さＬは、実際の移動経路と理論上の移動経路との差を表しているので、この長さＬには、サーボ遅延量に基づく変位は含まれない。したがって、この長さＬにより、実際値と指令値の関係が適正か否かをより正確に判断することができる。そして、次のステップＳ１５３では、この距離Ｌが所定の上限値Ｌ_MAX 以下であればステップＳ１５４へ、そうでなければステップＳ１９へ処理を移し、非常停止信号を出力する。
この様な判定を行えば、このサーボ制御における許容遅れ時間が大きな場合にも、実際座標Ａ（ｎ）に対する指令座標Ｂの妥当性をより正確に判定することができる。また、理論上の動作経路に対する実際の動作経路の許容変位（非常停止信号を出力しない許容範囲）を、上限値Ｌ_MAX により直視的に設定することができる。

〔その他の変形例〕
本発明の実施形態は、上記の形態に限定されるものではなく、その他にも以下に例示される様な変形を行っても良い。この様な変形や応用によっても、本発明の作用に基づいて本発明の効果を得ることができる。
（変形例１）
例えば、上記の実施例１、２では、図３のステップＳ１３〜Ｓ１５において、指令値と実際値との関係が適正か否かの判定を直交座標系における指令座標と実際座標とで評価したが、直接、６軸の角度として与える指令値と、角度して検出される実際値との偏差を演算し、６軸のうちの何れか１つの軸の偏差が閾値を越える場合には、非常停止信号を出力するようにしても良い。

（変形例２）
また、上記実施例１、２において、（１）及び（２）式における閾値δ_x1，δ_x2、Ｄを指令座標の時間変化量から得られる指令速度の大きさに比例して設定しても良い。これにより指令座標と実際座標との偏差の大きさの評価に、サーボ遅延量による影響を排除することができる。同様に、変形例１において、各軸の指令値と実際値との偏差を大きさを評価する閾値を、指令値の時間変化量から得られる指令速度に比例した値に設定しても良い。また、これらの閾値を、指令値、指令座標から得られる指令速度ではなく、実際値、実際座標から得られる実際速度に比例した値に設定するようにしても良い。

実施例１に係るロボット駆動制御システムの主要構成を示す制御ブロックダイヤグラム 同実施例に係るロボット駆動制御システムの運転の準備処理手順を示すフローチャート 同実施例に係る安全監視装置の監視処理手順を例示するフローチャート 同実施例に係る動作可能領域を設定する手法を示した説明図。 同実施例に係る動作可能領域を記憶したメモリを示した構成図。 同実施例に係る動作可能領域を設定する他の手法を示した説明図。 同実施例に係る他の動作可能領域を記憶したメモリを示した構成図。 本発明の実施例２に係る安全監視装置の監視処理手順を例示するフローチャート 実施例２における指令経路Γと実際座標Ａとの距離を求める方法を例示するグラフ

符号の説明

１ ： ロボット駆動制御システム
１００ ： 安全監視装置
１０１ ： 入力インターフェイス
１０２ ： 入力インターフェイス
１０３ ： 出力インターフェイス
１０４ ： メモリ
２１０ ： ＰＬＣ
２２０ ： ロボット制御装置
２３０ ： ロボット安全ユニット
３００ ： サーボユニット
ＭＳ ： マグネットスイッチ
α ： 実際値
β ： 指令値

Claims (8)

1. ロボットの教示基準点の移動経路が安全な所定の範囲内にあるか否かを監視するロボット安全監視装置において、
   前記ロボットを制御するロボット制御装置から出力された、前記ロボットの各軸を駆動するモータの動作変位量に関する指令値を入力する指令値入力手段と、
   前記動作変位量の変化に伴って運動する前記ロボットの教示基準点の実空間上における指令座標を前記指令値に基づいて算出する指令座標算出手段と、
   前記実空間上において前記教示基準点が位置することが許される所定の動作許可領域を記憶する動作許可領域記憶手段と、
   前記指令座標算出手段によって算出された前記指令座標が、前記動作許可領域記憶手段に記憶されている前記動作許可領域内に存在するか否かを判定する第１の判定手段と、
   前記第１の判定手段によって前記指令座標が前記動作許可領域内に存在しないと判定された場合には、前記ロボットを安全な状態にするための非常停止信号を出力する非常停止信号出力手段と
   を有することを特徴とするロボット安全監視装置。
2. 前記ロボットの各軸を駆動するモータの前記動作変位量に関する実際値を入力する実際値入力手段と、
   前記実際値と前記指令値との関係が適正であるか否かを判定する第２の判定手段と
   を有し、
   前記非常停止信号出力手段は、前記第１の判定手段によって前記指令座標が前記動作許可領域内に存在しないと判定された場合、又は、前記第２の判定手段によって前記実際値と前記指令値との関係が適正でないと判定された場合の何れかの場合には、前記ロボットを安全な状態にするための非常停止信号を出力する
   ことを特徴とするロボット安全監視装置。
3. 前記教示基準点の前記実空間上における実際座標を前記実際値に基づいて算出する実際座標算出手段を有し、
   前記第２の判定手段は、前記指令座標と前記実際座標との偏差に基づいて、前記実際値と前記指令値との関係が適正か否かを判定する
   ことを特徴とする請求項２に記載のロボット安全監視装置。
4. 前記教示基準点の実空間上における実際座標を前記実際値に基づいて算出する実際座標算出手段を有し、
   前記第２の判定手段は、前記実際座標に対応する時刻から過去所定数分の前記指令座標から得られる指令経路と、現在の前記実際座標との間の距離に基づいて、前記実際値と前記指令値との関係が適正か否かを判定する
   ことを特徴とする請求項２に記載のロボット安全監視装置。
5. 前記第２の判定手段は、前記指令値の単位時間当たりの変位から得られる各軸の指令速度と所定遅延時間との積を閾値として、前記実際値と前記指令値との関係が適正か否かを判定することを特徴とする請求項２に記載のロボット安全監視装置。
6. 前記第２の判定手段は、前記指令座標の単位時間当たりの変位から得られる指令座標の指令速度と所定遅延時間との積を閾値として、前記実際値と前記指令値との関係が適正か否かを判定することを特徴とする請求項３又は請求項４に記載のロボット安全監視装置。
7. 前記指令座標算出手段によって算出された前回の前記指令座標と今回の前記指令座標との差分に基づいて得られる指令速度が、所定の適正速度範囲内に存在するか否かを判定する第３の判定手段を有し、
   前記非常停止信号出力手段は、前記第１の判定手段によって前記指令座標が前記動作許可領域内に存在しないと判定された場合、前記第２の判定手段によって前記実際値と前記指令値との関係が適正でないと判定された場合、又は、前記第３の判定手段によって前記指令速度が前記適正速度範囲内に存在しないと判定された場合の何れか一つの場合には、前記ロボットを安全な状態にするための非常停止信号を出力する
   ことを特徴とする請求項２乃至請求項６の何れか１項に記載のロボット安全監視装置。
8. 産業用ロボットを駆動制御するロボット駆動制御システムにおいて、請求項１乃至請求項７の何れか１項に記載のロボット安全監視装置を有することを特徴とするロボット駆動制御システム。

Images