JP5365524B2 - ロボット動作規制方法およびロボットシステム - Google Patents

Description

本発明は、ロボット動作規制方法およびロボットシステムに関する。

ロボット、特に産業用ロボットは自動車の組み立て工場などで広く用いられている。
動作に際し、ロボットアームおよびその手首（そして、手首に備えられたワークやツール）は、制御装置のメモリに予め教示し記録された作業プログラム（ロボットを動作させ作業を実行するための手順が記述されたもの）により、周辺機器と干渉しないように、また無駄な動きにならないよう、所望の移動軌跡に沿って動作するようにされている。
しかし、ロボットアームや手首の動作により作業者などを危険に曝すような不測の事態が生じないように、ロボットの前記移動軌跡の外側には所定のマージンをおいて安全防護柵が配置されるのが通常である。
この安全防護柵は、ロボットの最大動作範囲の外部に設けられるものであるが、例えば、小物部品の搬送用途などでは、ロボットの作業のための動作範囲が狭いにもかかわらず、ロボットの最大動作領域の外部に安全防護柵を設けることは、ロボットの占有領域を広く確保することとなり、無駄である。そこで、ロボットの動作範囲をコンピュータによる制御で制限することも行われている。
例えば、特許文献１には、ロボット動作を規制するための領域を「仮想安全柵」としてメモリー上に定義し、ワークやツールを含むロボットの一部を内包する３次元空間領域を少なくとも２箇所以上定義し、この３次元空間領域の軌跡計算上での予測位置と仮想安全柵との照合を行い、一部でも仮想安全柵に接する場合に、ロボットを停止させる制御を行うことを特徴とするロボット動作規制方法が記載されている。
また、このような動作領域の監視の具体的な手段としては、例えば特許第３９３７０８０号（以降、参考文献１と称す）に、直方体として定義された干渉領域を、直方体の各面の４つの頂点を通る６つの外接球として内部表現し、外接球の中心位置とアーム各部との距離を求め、アームに設定した半径と外接球の半径から、干渉状態にあるかどうかを判断するという方法が提案されている。この方法によれば、コントローラのＣＰＵにさほど負荷を与えることなく、リアルタイムでの干渉領域チェックが可能である。
これらの技術を用いることにより、安全防護柵などに頼らずにロボットの動作範囲を制限することが可能となり、安全防護柵をより狭く設置して、工場などの限られた床面積を有効に活用することが可能となる。
特開２００４−３２２２４４号公報（第６−９頁、図１）

特許文献１は、軌跡計算上での予測位置が仮想安全柵を越える場合に、ロボットを停止させるものである。そして、ロボットの惰走については、「惰送、制動のための時間（距離）が必要となるが、先読みによりロボット自体が仮想安全柵よりも所定距離前方位置を制動開始位置として認識できるので、安全に停止が可能となる」とある。（００１３段落参照。なお、緊急停止時に惰性によって生じる行き過ぎ量について、特許文献１では「惰送」と記されているが、本願においては、「惰走」で統一する）
しかし、特許文献１では、結論から言うと惰走距離について考慮されていない。ここで充分に余裕を持った先読みを実時間で実施しようとした場合、非常に高いＣＰＵ能力が必要となり現実的ではない。
また、軌跡計算上は仮想安全柵に接触しないような動作パターンでも、何らかの異常事態（例えばライン非常停止や、サーボアンプの故障など）にて緊急停止した場合、各軸の惰走角度は、慣性力により生じる力のモーメントや重力モーメントの大小、ブレーキ能力の差異により、軸ごとに異なるため、惰走した位置は、本来想定されている動作軌跡から逸脱する。よってこの位置が仮想安全柵を越えてしまう可能性がある。
よって、ロボットを導入しようとするときに、この惰走量も考慮に入れた上で、安全防護柵と仮想安全柵の間のマージンを大きく確保する必要があり、スペースに無駄が生じることとなる。
本発明はこのような問題点に鑑みてなされたものであり、仮に緊急停止された場合に、各軸が惰走した位置が、動作禁止領域に進入する可能性がある場合に、その時点でロボットを停止させるか、または警告を発し、作業プログラムの修正を促すことにより、いかなる場合でもロボットが動作禁止領域に進入する事がないようにする。この作用によって、工場などの床面積や空間を無駄のない状態で有効に活用できるようにした、ロボット動作規制方法およびロボットシステムを提供することを目的とする。

上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、ロボットのアームと手首に備えたツール又は把持したワークを含めたアーム占有領域と、前記アーム占有領域が進入してはならない動作禁止領域と、をメモリー上に定義し、前記ロボットの各軸に対して設定され、前記ロボットを緊急停止した場合に、前記各軸を駆動するモータに備わるブレーキによって前記各軸に生じるブレーキングトルクを予め記憶しておき、演算周期毎に前記ワーク又はツールの目標位置を演算して前記ロボットの各軸の動作指令を生成する時に、次回の演算周期の前記ツール又はワークの目標位置に基づく前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、進入が確認された場合には、前記ロボットの動作を停止する制御を行ない、進入が確認されなかった場合には、現在位置における各軸の角度と次回目標位置における各軸の角度との差分を演算周期時間で除することによって各軸の速度を算出し、次回目標位置における各軸周りの慣性行列及び重力モーメントを算出し、前記各軸の速度及び前記慣性行列を用いて各軸に発生する運動エネルギーを算出し、前記重力モーメントを用いて各軸に発生する位置エネルギーを算出し、前記運動エネルギーに前記位置エネルギーを加算した値を前記ブレーキングトルクで除することによって、前記次回の演算周期の前記ツール又はワークの目標位置への動作指令に基づく前記ロボットの動作中に前記ロボットを緊急停止した場合の前記ロボットの各軸の惰走角度を推定し、各軸の惰走角度を次回の演算周期の前記各軸の前記動作指令に加算することで、前記ツール又はワークが目標位置に到達するまでの想定されている前記ロボットの動作軌跡から逸脱する前記ロボットの惰走予測位置を求め、前記惰走予測位置における前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、進入が確認された場合には、前記ロボットの動作を停止する制御を行なうことを特徴とするものである。
また、請求項２に記載の発明は、ロボットと、前記ロボットを制御するロボット制御装置と、を有し、前記ロボット制御装置は、前記ロボットのアーム又は前記ロボットの手首に備わるツール若しくは把持したワークに基づくアーム占有領域と前記アーム占有領域が進入してはならない動作禁止領域とを格納するとともに、前記ロボットの各軸に対して設定され、前記ロボットを緊急停止した場合に、前記各軸を駆動するモータに備わるブレーキによって前記各軸に生じるブレーキングトルクを予め記憶するメモリーと、演算周期毎に前記ツール又はワークの動作目標位置を求め前記ロボットの各軸の動作指令を生成する目標位置算出部と、前記目標位置算出部で求めた前記ツール又はワークの動作目標位置に基づく前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、進入が確認された場合には、前記ロボットの動作を停止する停止要求を出力する動作禁止領域進入監視部と、前記ロボットの動作を行なう駆動部と、を備え、前記停止要求で前記ロボットの動作を停止制御するロボットシステムにおいて、前記ロボット制御装置は、現在位置における各軸の角度と次回目標位置における各軸の角度との差分を演算周期時間で除することによって各軸の速度を算出し、次回目標位置における各軸周りの慣性行列及び重力モーメントを算出し、前記各軸の速度及び前記慣性行列を用いて各軸に発生する運動エネルギーを算出し、前記重力モーメントを用いて各軸に発生する位置エネルギーを算出し、前記運動エネルギーに前記位置エネルギーを加算した値を前記ブレーキングトルクで除することによって、前記ツール又はワークの動作目標位置へ前記ロボットを動作中に前記ロボットを緊急停止した場合の前記ロボットの各軸の惰走角度を推定し、前記推定した各軸の惰走角度を前記各軸の動作指令に加算することで、前記ツール又はワークが動作目標位置に到達するまでの想定されている前記ロボットの動作軌跡から逸脱する前記ロボットの惰走予測位置を求める惰走予測位置算出部を備え、前記動作禁止領域進入監視部は、前記惰走予測位置における前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、進入が確認された場合には、前記ロボットの動作を停止する停止要求を更に出力することを特徴とするものである。
また、請求項３に記載の発明は、前記ロボット制御装置は表示装置を備える教示具と接続され、前記ロボットの動作を停止する停止要求で停止制御を行う時に、合わせて前記ロボットを停止する旨の表示を行うことを特徴とするものである。
また、請求項４に記載の発明は、前記ロボット制御装置は表示装置を備える教示具と接続され、予め定められた制御方式が継続の設定であれば、前記停止要求で停止制御を行わず、前記ロボットを継続制御する旨の表示を行うことを特徴とするものである。
また、請求項５に記載の発明は、ロボットと、前記ロボットを制御するロボット制御装置と、を有し、前記ロボット制御装置は、演算周期毎に手首に備えたツール又は把持したワークの動作目標位置を求め前記ロボットの各軸の動作指令を生成する目標位置算出部と、前記動作指令に基づいて前記ロボットの動作を行なう駆動部と、を備えたロボットシステムにおいて、前記ロボットのアーム若しくは前記ロボットの手首に備えたツール又は把持したワークに基づくアーム占有領域と前記アーム占有領域が進入してはならない動作禁止領域とを格納するとともに、前記ロボットの各軸に対して設定され、前記ロボットを緊急停止した場合に、前記各軸を駆動するモータに備わるブレーキによって前記各軸に生じるブレーキングトルクを予め記憶するメモリーと、前記ロボットの各軸に備わる位置検出器からモータの現在位置を読み取り、モータの現在位置から前記ロボットの現在位置を求め、モータの現在位置及びロボットの現在位置を記憶する現在位置検出部と、前記目標位置算出部で求めた前記ツール又はワークの動作目標位置に基づく前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、進入が確認された場合には、前記ロボットの動作を停止する停止要求を出力する動作禁止領域進入監視部と、前回位置における各軸の角度と現在位置における各軸の角度との差分を演算周期時間で除することによって各軸の速度を算出し、現在位置における各軸周りの慣性行列及び重力モーメントを算出し、前記各軸の速度及び前記慣性行列を用いて各軸に発生する運動エネルギーを算出し、前記重力モーメントを用いて各軸に発生する位置エネルギーを算出し、前記運動エネルギーに前記位置エネルギーを加算した値を前記ブレーキングトルクで除することによって、前記ツール又はワークの動作目標位置へ前記ロボットを動作中に前記ロボットを緊急停止した場合の前記ロボットの各軸の惰走角度を推定し、前記推定した各軸の惰走角度を前記各軸の動作指令に加算することで、前記ツール又はワークが動作目標位置に到達するまでの想定されている前記ロボットの動作軌跡から逸脱する前記ロボットの惰走予測位置を求める惰走予測位置算出部と、を有し、前記動作禁止領域進入監視部は、前記惰走予測位置における前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、進入が確認された場合には、前記ロボットの動作を停止する停止要求を更に出力する動作領域監視装置を備えることを特徴とするものである。
また、請求項６に記載の発明は、前記メモリー、前記現在位置検出部、前記惰走予測位置算出部及び前記動作禁止領域進入監視部は前記ロボット制御装置とは独立していることを特徴とするものである。

請求項１、２及び５に記載の発明によると、予期しない緊急停止が起こった場合のロボットの惰走も考慮して動作領域の制限が可能となるため、いかなる場合でもロボットが動作禁止領域に進入する事がなくなるので、安全に、正確な動作禁止領域を設定することが可能となる。従って、そこで働く作業員の安全性を確保した上で、工場の床面積や空間を、無駄のない状態で有効に活用することができる。
また、請求項３及び４に記載の発明によると、動作制限の状態を作業者が確認することができ、作業者による確認を容易化することができる。
また、請求項５及び６に記載の発明によると、ロボットの動作監視する動作領域監視装置は、ロボットを制御するロボット制御装置より独立した構成となるので、ロボットの制御に関する改善（バージョンアップ）などを行っても、動作領域監視装置は影響を受けない構成となる。動作領域監視での安全性は、ロボットの制御に関係なく維持することができるので、例えば動作領域監視装置を第三者機関による安全認証取得などで顧客の信頼を得ることが容易にできる。

本発明によるロボット動作規制方法と装置方法を備えたロボットシステムを説明する図 本発明によるロボット動作規制方法を実現するためのブロック図 本発明によるロボット動作規制方法と装置方法を備えたロボットの動作を説明する図 本発明によるロボット動作規制方法を実現するためのフローチャート 動作禁止領域を用いて、動作可能領域を定義する方法を説明する図 本発明の第２の方法によるロボット動作規制方法を実現するためのブロック図 本発明の第２の方法によるロボット動作規制方法を実現するためのフローチャート

符号の説明

１ ロボット
２ 本体
３、４、５ アーム
６ 把持装置
７ ツール
８ 関節
９ ワーク
１０ 物理的な安全防護柵
２０ 制御装置
２１ 教示具
５０ 動作禁止領域
５１ 動作禁止領域（１）
５２ 動作禁止領域（２）
５３ 動作禁止領域（３）
５４ 動作禁止領域（４）
７０ 動作可能領域
２０１ 教示・操作部
２０２ 教示データ格納エリア
２０３ パラメータ格納エリア
２０４ 動作指令生成部
２０５ 駆動部
２０６ 次回目標位置算出部
２０７、６０３ 惰走予測位置算出部
２０８、６０４ 動作禁止領域進入監視部
３０１ 現在位置
３０２ 次回目標位置
３０３ 惰走予測位置
３０４ 第１軸
６０１ 動作領域監視装置
６０２ 現在位置検出部
６０５ モータ位置
６０６ 緊急停止指令
Ａ１、Ａ２、Ａ３、Ａ４、Ａ５、Ａ６ アーム占有領域

以下、本発明の方法の具体的実施例について、図に基づいて説明する。

図１は、本発明によるロボット動作規制方法と装置方法を備えたロボットのシステムを説明する図である。（ロボットはワイヤー形態で示している。）工場のフロアに物理的な安全防護柵１０が設けてあり、その中にロボット１が配置される。
この例で、ロボット１は、本体２と３本のアーム３、４、５を備えている。アーム５には把持装置６を介してツール７が備えられる。ツール７として、アーク溶接用の溶接トーチや、スポット溶接用の溶接ガン、搬送用途におけるハンドなどが取り付けられる。アーム３、アーム４、アーム５は、それぞれ関節８で接続されている。
９は床に置かれたワークである。ワーク９は、被溶接体や搬送物などである。
本体２には制御装置２０から必要な信号が送られ、所定の作業プログラムに従い、アーム３、４、５は所定の動作を行い、把持装置６又はツール７は所望の軌跡に沿った動作を行う。
制御装置２０には教示具２１が接続されていて、ロボット１への教示や作業プログラムの書き換えなどを行なう。

ロボットを導入し、設置して稼動に先立って、ロボット１の動作禁止領域５０を設定する。この動作禁止領域５０の設定は、設置のときのほかに、変更を要する際に随時行われる。動作禁止領域５０は、直方体として定義され設定される。設定は教示具２１を用いて、直方体の頂点の座標値を数値入力するか、あるいは教示具２１を操作してロボット１の制御点を動作し、直方体の頂点の位置を指定することによって定義する。定義された動作禁止領域５０は、制御装置２０のメモリー上に格納される。なお、動作禁止領域５０は、複数個の領域として定義することも可能である。

また、ロボット１の各アーム３、４、５やツール７が空間中に占める領域を、アーム占有領域Ａ１、Ａ２、Ａ３、Ａ４、Ａ５、Ａ６として定義する。
まず、アーム３、４、５を、関節８をつなぐ直線を軸心とする所定半径の円筒領域として定義し、それぞれをＡ４、Ａ５、Ａ６とする。
また、ロボット１の各関節８を内包する領域Ａ１、Ａ２は、関節８の軸上の１点を中心とする所定半径の球として定義する。関節８の軸上の１点は、通常は前記Ａ４、Ａ５、Ａ６の領域を定義する際に使用される「関節８をつなぐ直線」と、関節８の軸線との交点を使用する。さらに、アーム５の先端の把持装置６及びツール７を内包する領域を、所定半径の球Ａ３として定義する。これらのアーム占有領域Ａ１ないしＡ６の定義も、制御装置２０のメモリー上に格納される。
いずれのアーム占有領域も、おおよその半径を持つ球および円筒であればよく、より半径の大きな球および円筒として定義する場合には、それだけ安全率は高くなるが、上記した動作禁止領域５０はより広い領域として設定することが必要となり、スペース効率が低下する。あるいはアームの移動に制限を受けるようになる。

図２は、制御装置２０にて構築されているロボット制御の一実施例を示すブロック図である。
教示・操作部２０１は、教示具２１における操作者の操作で、作業プログラムの呼出し・実行、及び教示作業のためのロボット操作を行なう。また、教示データ格納エリア２０２への教示データ（作業プログラムやその他の作業に関連する情報）の格納や、パラメータ格納エリア２０３への各種パラメータの格納なども行なう。パラメータ格納エリア２０３には、補間演算のために必要なアーム各部の寸法や、ロボット軸動作のために必要な、減速比やモータ定数などの軸仕様、及び各軸アーム占有領域Ａ１ないしＡ６の半径及び動作禁止領域５０を定義するための座標値等も格納される。

操作者が作業プログラムの呼出し・実行、または、教示作業のためのロボット操作を行なうと、教示・操作部２０１は、動作指令生成部２０４に対して、ロボット動作指令要求を送る。動作指令生成部２０４は、ロボット動作指令要求に対し、作業プログラムで定められたロボットの動作軌跡上の補間点を次回目標位置として決められた補間周期毎に、次回目標位置算出部２０６にて算出する。ここで求めた次回目標位置について、動作禁止領域進入監視部２０８にて、動作禁止領域に進入しないかどうかのチェックを行なう。また、次回目標位置に動作している状態で緊急停止をかけられた場合に惰走して停止する位置（惰走予測位置）を、惰走予測位置算出部２０７によって算出し、求められた惰走予測位置について、動作禁止領域進入監視部２０８にて、動作禁止領域に進入しないかどうかのチェックを行なう。次回目標位置算出部２０６は、算出した位置へ動作するためのロボット各軸の指令値を、駆動部２０５へ送る。しかし、動作禁止領域進入監視部２０８において、動作禁止領域への進入が検知された場合は、停止要求が駆動部２０５へ送られる。
駆動部２０５は、ロボット１の各軸を、動作指令生成部２０４から送られた指令値に動作させるが、停止要求を入信すると、ロボット１の動作を行わず、停止させる。

図３は、本発明の基本的な考え方について説明するためのロボットの動作を説明する図である。
ロボット１は、現在位置３０１から、次回目標位置３０２へ動作しようとしている。ここで、ロボット１に緊急停止をかけると、それぞれの軸は、その時の負荷状況に応じて僅かなりとも惰走して停止する。各軸について惰走角度を予測して、次回目標位置に加算することで、惰走予測位置３０３を求めることが出来る。
図３（ｂ）において、θs1は、現在位置３０１における第１軸３０４の角度、θe1は、次回目標位置３０２における第１軸３０４の角度を表している。θd1は、次回目標位置３０２の姿勢における負荷状況において、緊急停止した場合の惰走角度を表している。ここで、θe1＋θd1を算出することにより、惰走予測位置３０３を求めることが出来る。
この惰走予測位置３０３において、アーム占有領域を考慮し、これが動作禁止領域５０に進入する場合には、ロボット１を次回目標位置３０２へ動作させるよりも先に、ロボット１を停止させることで、惰走しても動作禁止領域５０に進入することを回避できる。
図３では、（ａ）よりも（ｂ）の方が、第１軸３０４における慣性力により生じる力のモーメント及び重力モーメントが大きい。よって、（ｂ）の方が、より大きな惰走角度を生じ、図に示したように、手首部のアーム占有領域が、動作禁止領域５０に進入することが予測できる。従って（ｂ）の場合には、ロボット１を停止させる。

図４は、動作指令生成部２０４において、動作禁止領域に干渉しないように制限されたロボットの動作を実現するためのフローチャートである。この図を用いて本発明の方法を順を追って説明する。
ここでは、軸数がｎ個であるロボットについて考慮するものとする。基本的には、各ステップにおいて、第１軸から第ｎ軸について同様の処理を繰り返す。各軸の軸番号を表す添字として、ｉを用いる。（例えば、６軸ロボットであれば、ｉ＝１・・・６を取る。）

［Ｓ０１］
ステップＳ０１では、次回目標位置算出部２０６において、演算周期毎の次回目標位置（図３の３０２）を算出する。この次回目標位置とは、ロボット各軸の角度まで求められたものを指す。
続いてステップＳ０２へ進む。

［Ｓ０２］
ステップＳ０２では、ステップＳ０１で求めた次回目標位置において、アーム占有領域Ａ１ないしＡ６が、動作禁止領域５０に進入していないかどうかのチェックを行なう。このチェックは、動作禁止領域進入監視部２０８において行なわれる。
ここでの進入監視の具体的な方法については、様々な方法があるが、ここでは、参考文献１のように、動作禁止領域を直方体として定義したものを、各面の４つの頂点を通る６つの外接球として表現し、その中心位置と、アーム各部との距離を求め、アームに設定した半径と外接球の半径から、干渉状態にあるかどうかを判断する方法を用いるものとする。
なお、動作禁止領域５０が、複数個の領域で定義されている場合、全ての動作禁止領域について同様のチェックを行ない、一つでも進入している場合には、「進入する」という判定とする。
ここで「進入する」と判断された場合は、ステップＳ１２に進む。そうでなければ、ステップＳ０３へ進む。

［Ｓ０３］
ステップＳ０３では、現在位置と次回目標位置の差分と、演算周期時間：ｔから、ロボット各軸の速度：ωｉを求める。
この速度の算出は、惰走予測位置算出部２０７において行なわれる。
例えば、図３（ｂ）における第１軸の速度ω１は、次式で求められる。
ω１＝（θe1−θs1）／ｔ
ここでθs1、θe1は、図３において説明したとおり、
θs1：現在位置での第１軸角度、 θe1：次回目標位置での第１軸角度
を表す。
同様にして、他の軸（２〜ｎ軸）についても、各軸速度ωｉを算出する。つまり、
ωｉ＝（θeｉ−θsｉ）／ｔ （ i = 1〜ｎ ）
続いてステップＳ０４へ進む。

［Ｓ０４］
ステップＳ０４では、惰走予測位置算出部２０７において、次回目標位置におけるロボットの各軸周りに発生する慣性行列：Ｊ、及び重力モーメント：Ｇを算出する。
軸数ｎのロボットの場合、慣性行列Ｊはｎ行ｎ列、重力モーメントＧは、ｎ行１列の行列式で表現される。

ロボット各軸の慣性行列や重力モーメントは、各アーム部を、関節を区切りとした個別のパーツに分割し、それぞれにおける質量及び重心位置、形状モーメントと、ロボット各軸角度から算出することが出来る。各アーム部の質量及び重心位置、形状モーメントは、パラメータ格納エリア２０３に予め格納しておく。
ロボットの姿勢から各軸の慣性行列や重力モーメントを算出する具体的な方法としては、様々な運動方程式があるので、これらの何れかを用いれば良い。例えば、「ニュートン・オイラーの運動方程式」や「ラグランジュの運動方程式」等の運動方程式を用いる方法が、一般的によく知られており、計算機上での実時間計算に向いている。
続いてステップＳ０５へ進む。

［Ｓ０５］
ステップＳ０５では、惰走予測位置算出部２０７において、ステップＳ０４までに求められた、各軸の速度ωｉ、慣性行列Ｊ、重力モーメントＧを用いて、各軸に発生するエネルギー：Ｕｉを算出する。
動作中の回転体に働く全エネルギーは、下記式にて表現される。
Ｕ＝Ｋ＋Ｐ
ここで、Ｕ：全エネルギー、Ｋ：運動エネルギー、Ｐ：位置エネルギーである。
ロボットアームは、殆どの場合、各軸を中心とした回転運動を行なう回転体である。運動エネルギー：Ｋ は、回転体の場合、下式で表現される。

一方、位置エネルギー：Ｐは、重力モーメント：Ｇと等しい。
従って、第ｉ軸のエネルギー：Ｕｉは、次式にて求めることが出来る。

ｉ＝１〜ｎの全軸について上記式の計算を行ない、各軸のエネルギーを算出する。
続いてステップＳ０６へ進む。

［Ｓ０６］
ステップＳ０６では、惰走予測位置算出部２０７で、ロボット１に対して緊急停止をかけた場合の、各軸の惰走角度を算出する。第ｉ軸に発生しているエネルギーと、第ｉ軸のブレーキング能力、及び各軸の惰走角度の間には、下式のような関係がある。
Ｕｉ＝Ｔbi・θdi
Ｔbi：第ｉ軸のブレーキングトルク θdi：第ｉ軸の惰走角度
ここで、ブレーキングトルク：Ｔbiとは、サーボモータに備わる機構式ブレーキの能力や、サーボアンプのダイナミックブレーキ、電源回生、減速機構部の摩擦抵抗などによって決まる、各軸固有の値である。ブレーキングトルクが大きいほど、各軸の惰走角度は小さくなる。
従って、あるエネルギーを持って動作しているロボット軸を緊急停止させた際の、第ｉ軸の惰走角度は、下式によって算出出来る。

この式を用いて、各軸の惰走角度θdiを算出する。ここで、例えば、図３（ｂ）のθd1は、第１軸における惰走角度を表している。
なお、各軸のブレーキングトルクの設定値は、教示具２１等によって、パラメータ格納エリア２０３に格納されている。
続いて、ステップＳ０７へ進む。

［Ｓ０７］
ステップＳ０７では、ステップＳ０６で求めた各軸の惰走角度に基づいて、「惰走予測位置」を算出する。つまりここで求めた惰走予測位置は、次回目標位置にて、ロボット１に緊急停止がかけられた場合にロボット１が行き着くと予想される位置である。図３（ｂ）の第１軸で説明すると、次回目標位置３０２における第１軸の角度θe1に対し、ステップＳ０６で求めた惰走角度θd1を加算した角度が、惰走予測位置３０３における第１軸の角度となる。
続いてステップＳ０８へ進む。

［Ｓ０８］
ステップＳ０８では、惰走予測位置３０３が、動作禁止領域に進入するかどうかのチェックを行なう。チェック方法自体は、ステップＳ０２と同様であり、このチェックは、動作禁止領域進入監視部２０８において行なわれる。
ここで「進入する」と判断された場合は、ステップＳ０９に進む。そうでなければ、ステップＳ１１へ進む。
［Ｓ０９］
ステップＳ０９では、予め操作者がパラメータに設定した制御方法に従い、停止又は継続の制御へ分岐する。制御方法が停止であれば、ステップＳ１２へ進み、継続であれば、ステップＳ１０へ進む。

［Ｓ１０］
ステップＳ１０では、教示具２１のディスプレイに、ステップＳ０８にて惰走予測位置が動作禁止領域に進入してしまう作業プログラムの教示ステップ番号と一緒に、「動作継続」又は動作継続の旨のメッセージを表示し、ステップＳ１１へ進む。
［Ｓ１１］
ステップＳ１１では、次回目標位置算出部２０６が算出した次回目標位置に動作するための動作指令を、駆動部２０５へ出力し、ロボット１を動作させる。
［Ｓ１２］
ステップＳ１２に到達した場合は、動作禁止領域進入監視部２０８が、駆動部２０５に対して、停止要求を送る。この停止要求で駆動部２０５はロボット１の動作を停止させる。また、停止理由をメッセージとして教示具２１のディスプレイに表示させる。
このような手順を取ることにより、仮に緊急停止が起こって、ロボット１が惰走したとしても、ロボット１が動作禁止領域に入り込むことがなくなる。

惰走予測位置は、実際に教示された軌跡とは異なる位置となるため、教示作業中には、惰走での干渉を予測することは困難である。
本実施例においては、惰走予測位置が動作禁止領域に進入すると予測された場合の制御手法として「緊急停止」と「動作継続」を選択できるようにしている。（ステップＳ０９）
これを活用して、作業プログラムを教示した後の動作確認の際には「動作継続」を指定しておいて作業プログラムを再生することにより、緊急停止で動作禁止領域への干渉の恐れがある箇所を確認し、教示点や指令速度の見直しを行なって、通常動作では干渉の恐れが無いようにする。これにより、予測が困難な惰走での干渉の可能性を極力排除することが出来る。
そして、実際の生産時には「緊急停止」を指定しておき、特異な状況にて干渉の恐れが発生した場合には、緊急停止させる、という活用が可能である。

本発明における動作禁止領域の設定では、使用形態において、ロボットが進入してはならない領域を定義するのではなく、ロボットが動作することを許可する領域、即ち「動作可能領域」を指定する方が簡便である場合も多い。つまり、安全防護柵で囲われた領域を指定するような場合である。
この様な場合は、定義された動作可能領域を取り囲むような、複数の動作禁止領域を定義することによって、同様の効果を得ることが可能である。
図５は、動作可能領域７０がロボット１を包含する直方体として定義されたシステムを、上面から見た様子を表している。
ユーザーが動作可能領域７０を定義した場合に、コントローラは自動的に、動作可能領域７０の各面に隣接する、複数の動作禁止領域５１〜５４を自動生成して、予めパラメータ格納エリア２０３に展開させるようにすれば良い。これにより、前記した方法を用いて、動作可能領域７０の外に出ないような動作規制を実現することが出来る。

前記のロボット動作規制方法は、ロボットの制御を行う制御プログラムで構成されるが、安全性及び信頼性を高めるために、動作禁止領域への進入を監視して、進入時にロボット動作を停止させる制御を行なうための装置を、独立させて設けることも出来る。
図６及び図７は、本発明の他の態様として、前記の監視及び停止制御を行なう装置を独立させた場合の実施形態を説明する図である。
図６は、図２のシステムに対し、動作領域監視装置６０１を追加した構成となっている。動作領域監視装置６０１は、駆動部２０５より、定められた監視周期毎に各軸モータ位置６０５をエンコーダ等の位置検出器から、現在位置検出部６０２が読み取る。この各軸のモータ位置６０５から、ロボットの現在位置（ワーク又はツールの位置、ＴＣＰとも言う。）を求め、このロボットの現在位置に対し、動作禁止領域進入監視部６０４にて、動作禁止領域への進入をチェックする。また、現在位置検出部６０２で読み取ったモータ位置６０５情報を元に、惰走予測位置算出部６０３において、この時点で緊急停止をかけられた場合に惰走して停止する位置を算出する。この惰走予測位置に対し、動作禁止領域進入監視部６０４にて、動作禁止領域への進入をチェックする。動作禁止領域進入監視部６０４は、動作禁止領域との進入を検知すると、駆動部２０５に対して、例えば駆動電源遮断信号などの緊急停止指令６０６を出力する。
図７は、図６のシステム構成において、動作禁止領域に干渉しないようにロボットの動作を制限することを実現するためのフローチャートである。この図を用いて本発明の方法を順を追って説明する。

［Ｓ１０１］
ステップＳ１０１で、現在位置検出部６０２にてロボットの各軸のモータ位置６０５を読み込む。次に、読み込んだ各モータ位置６０５からロボットの現在位置を求める。以降で行う処理のために、各軸モータの現在の位置（現在位置）は、前回読み込んだ前回の位置（前回位置）と共に記憶しておく。続いてステップＳ１０２へ進む。
［Ｓ１０２］
ステップＳ１０２では、動作禁止領域進入監視部６０４にて、ステップＳ１０１で求めたロボットの現在位置において、アーム占有領域Ａ１ないしＡ６が、動作禁止領域５０に進入していないかどうかのチェックを行なう。具体的な進入監視の方法は、前記図４のフローチャートのステップＳ０２で行なっていたのと同様の手法が適用できる。
ここで「進入する」と判断された場合は、ステップＳ１０９に移行する。そうでなければ、ステップＳ１０３へ進む。
［Ｓ１０３］
ステップＳ１０３では、各軸モータの前回位置と現在位置の差分と、監視周期時間から、ロボット各軸の速度：ωｉを求める。ｉは各軸の軸番号を表す添字である。続いてステップＳ１０４へ進む。

［Ｓ１０４］
ステップＳ１０４では、各軸モータの現在位置における、ロボットの各軸周りに発生する慣性行列：Ｉ、及び重力モーメント：Ｇを算出する。これらの算出は、惰走予測位置算出部６０３にて行なわれる。その方法は、図４のステップＳ０４と同じである。
続いてステップＳ１０５へ進む。
［Ｓ１０５］
ステップＳ１０５では、ステップＳ１０４までに求められた、各軸の速度ωｉ、慣性行列Ｉ、重力モーメントベクトルＧを用いて、各軸に発生するエネルギー：Ｕｉを算出する。これらの算出は、惰走予測位置算出部６０３にて行なわれる。算出方法は、図４のステップＳ０５と同じである。続いてステップＳ１０６へ進む。
［Ｓ１０６］
ステップＳ１０６では、各軸モータの現在位置からロボットに緊急停止をかけた場合の、各軸の惰走角度を算出する。これらの算出は、惰走予測位置算出部６０３にて行なわれる。算出方法は、図４のステップＳ０６と同じである。続いて、ステップＳ１０７へ進む。
［Ｓ１０７］
ステップＳ１０７では、各軸モータの現在位置に、ステップＳ１０６で求めた各軸の惰走角度を加算して、「惰走予測位置」を算出する。これらの算出は、惰走予測位置算出部６０３にて行なわれる。算出方法は、図４のステップＳ０７と同じである。続いて、ステップＳ１０８へ進む。

［Ｓ１０８］
ステップＳ１０８では、惰走予測位置が、動作禁止領域に進入するかどうかのチェックを行なう。チェック方法自体は、ステップＳ１０２と同様である。
ここで「進入する」と判断された場合は、ステップＳ１０９に進む。そうでなければ、今回の監視周期における監視処理は終了する。
［Ｓ１０９］
ステップＳ１０９では、駆動部２０５に対して、緊急停止要求を指令する。
このような構成及び手順を取ることにより、仮に動作指令生成部が故障して、ロボットに異常な指令が送信された場合でも、惰走位置が動作禁止領域に入り込まないうちにロボットを停止させることが出来る。

本発明は、ロボットの動作予測を行って、仮想の安全防護柵である動作禁止領域への進入を静的及び動的に監視するものであるため、信頼性の高いロボットの動作領域制限を行うころが可能となる。ロボットの意図しない動作領域の逸脱を頑丈な安全防護柵で防いでいたが、信頼性が増すので、簡単な柵等でロボットの動作領域を示すことができるので、ロボット導入準備を簡素化することができる。

Claims (6)

1. ロボットのアームと手首に備えたツール又は把持したワークを含めたアーム占有領域と、
   前記アーム占有領域が進入してはならない動作禁止領域と、をメモリー上に定義し、
   前記ロボットの各軸に対して設定され、前記ロボットを緊急停止した場合に、前記各軸を駆動するモータに備わるブレーキによって前記各軸に生じるブレーキングトルクを予め記憶しておき、
   演算周期毎に前記ワーク又はツールの目標位置を演算して前記ロボットの各軸の動作指令を生成する時に、
   次回の演算周期の前記ツール又はワークの目標位置に基づく前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、
   進入が確認された場合には、前記ロボットの動作を停止する制御を行ない、
   進入が確認されなかった場合には、
   現在位置における各軸の角度と次回目標位置における各軸の角度との差分を演算周期時間で除することによって各軸の速度を算出し、次回目標位置における各軸周りの慣性行列及び重力モーメントを算出し、前記各軸の速度及び前記慣性行列を用いて各軸に発生する運動エネルギーを算出し、前記重力モーメントを用いて各軸に発生する位置エネルギーを算出し、前記運動エネルギーに前記位置エネルギーを加算した値を前記ブレーキングトルクで除することによって、前記次回の演算周期の前記ツール又はワークの目標位置への動作指令に基づく前記ロボットの動作中に前記ロボットを緊急停止した場合の前記ロボットの各軸の惰走角度を推定し、
   各軸の惰走角度を次回の演算周期の前記各軸の前記動作指令に加算することで、前記ツール又はワークが目標位置に到達するまでの想定されている前記ロボットの動作軌跡から逸脱する前記ロボットの惰走予測位置を求め、
   前記惰走予測位置における前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、
   進入が確認された場合には、前記ロボットの動作を停止する制御を行なうことを特徴とするロボット動作規制方法。
2. ロボットと、前記ロボットを制御するロボット制御装置と、を有し、
   前記ロボット制御装置は、
   前記ロボットのアーム又は前記ロボットの手首に備わるツール若しくは把持したワークに基づくアーム占有領域と前記アーム占有領域が進入してはならない動作禁止領域とを格納するとともに、前記ロボットの各軸に対して設定され、前記ロボットを緊急停止した場合に、前記各軸を駆動するモータに備わるブレーキによって前記各軸に生じるブレーキングトルクを予め記憶するメモリーと、
   演算周期毎に前記ツール又はワークの動作目標位置を求め前記ロボットの各軸の動作指令を生成する目標位置算出部と、
   前記目標位置算出部で求めた前記ツール又はワークの動作目標位置に基づく前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、進入が確認された場合には、前記ロボットの動作を停止する停止要求を出力する動作禁止領域進入監視部と、
   前記ロボットの動作を行なう駆動部と、を備え、
   前記停止要求で前記ロボットの動作を停止制御するロボットシステムにおいて、
   前記ロボット制御装置は、
   現在位置における各軸の角度と次回目標位置における各軸の角度との差分を演算周期時間で除することによって各軸の速度を算出し、次回目標位置における各軸周りの慣性行列及び重力モーメントを算出し、前記各軸の速度及び前記慣性行列を用いて各軸に発生する運動エネルギーを算出し、前記重力モーメントを用いて各軸に発生する位置エネルギーを算出し、前記運動エネルギーに前記位置エネルギーを加算した値を前記ブレーキングトルクで除することによって、前記ツール又はワークの動作目標位置へ前記ロボットを動作中に前記ロボットを緊急停止した場合の前記ロボットの各軸の惰走角度を推定し、前記推定した各軸の惰走角度を前記各軸の動作指令に加算することで、前記ツール又はワークが動作目標位置に到達するまでの想定されている前記ロボットの動作軌跡から逸脱する前記ロボットの惰走予測位置を求める惰走予測位置算出部を備え、
   前記動作禁止領域進入監視部は、
   前記惰走予測位置における前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、
   進入が確認された場合には、前記ロボットの動作を停止する停止要求を更に出力することを特徴とするロボットシステム。
3. 前記ロボット制御装置は表示装置を備える教示具と接続され、前記ロボットの動作を停止する停止要求で停止制御を行う時に、合わせて前記ロボットを停止する旨の表示を行うことを特徴とする請求項２に記載のロボットシステム。
4. 前記ロボット制御装置は表示装置を備える教示具と接続され、予め定められた制御方式が継続の設定であれば、前記停止要求で停止制御を行わず、前記ロボットを継続制御する旨の表示を行うことを特徴とする請求項２に記載のロボットシステム。
5. ロボットと、前記ロボットを制御するロボット制御装置と、を有し、
   前記ロボット制御装置は、
   演算周期毎に手首に備えたツール又は把持したワークの動作目標位置を求め前記ロボットの各軸の動作指令を生成する目標位置算出部と、
   前記動作指令に基づいて前記ロボットの動作を行なう駆動部と、を備えたロボットシステムにおいて、
   前記ロボットのアーム若しくは前記ロボットの手首に備えたツール又は把持したワークに基づくアーム占有領域と前記アーム占有領域が進入してはならない動作禁止領域とを格納するとともに、前記ロボットの各軸に対して設定され、前記ロボットを緊急停止した場合に、前記各軸を駆動するモータに備わるブレーキによって前記各軸に生じるブレーキングトルクを予め記憶するメモリーと、
   前記ロボットの各軸に備わる位置検出器からモータの現在位置を読み取り、モータの現在位置から前記ロボットの現在位置を求め、モータの現在位置及びロボットの現在位置を記憶する現在位置検出部と、
   前記目標位置算出部で求めた前記ツール又はワークの動作目標位置に基づく前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、進入が確認された場合には、前記ロボットの動作を停止する停止要求を出力する動作禁止領域進入監視部と、
   前回位置における各軸の角度と現在位置における各軸の角度との差分を演算周期時間で除することによって各軸の速度を算出し、現在位置における各軸周りの慣性行列及び重力モーメントを算出し、前記各軸の速度及び前記慣性行列を用いて各軸に発生する運動エネルギーを算出し、前記重力モーメントを用いて各軸に発生する位置エネルギーを算出し、前記運動エネルギーに前記位置エネルギーを加算した値を前記ブレーキングトルクで除することによって、前記ツール又はワークの動作目標位置へ前記ロボットを動作中に前記ロボットを緊急停止した場合の前記ロボットの各軸の惰走角度を推定し、前記推定した各軸の惰走角度を前記各軸の動作指令に加算することで、前記ツール又はワークが動作目標位置に到達するまでの想定されている前記ロボットの動作軌跡から逸脱する前記ロボットの惰走予測位置を求める惰走予測位置算出部と、を有し、
   前記動作禁止領域進入監視部は、前記惰走予測位置における前記アーム占有領域が前記動作禁止領域に進入しないかを確認し、進入が確認された場合には、前記ロボットの動作を停止する停止要求を更に出力する動作領域監視装置を備えることを特徴とするロボットシステム。
6. 前記メモリー、前記現在位置検出部、前記惰走予測位置算出部及び前記動作禁止領域進入監視部は前記ロボット制御装置とは独立していることを特徴とする請求項５記載のロボットシステム。