JP7488474B2 - ロボット制御装置 - Google Patents

Description

本実施形態は、ロボット制御装置に関する。

例えば多関節型のロボットを限られたスペースで使用するために、ロボットの可動範囲内に人等の侵入を許すことがある。このような場合、従来構成では、ロボットが人等に接触することを防止するために、ロボットの可動範囲内において人等の侵入を禁止する領域と人等が侵入可能な領域とを区画し、その領域の間に柵を設けることが行われる。なお、以下の説明では、人等の侵入を禁止する領域をロボットの動作領域と称し、人等が侵入可能な領域を指定領域と称する。

このような柵は、ロボットが万一暴走して柵に衝突した場合であっても柵の外側つまり指定領域内に存在する人等にロボットが接触して危害が及ばないように、ロボットの衝突に耐え得るような頑丈なものである必要があることから、大掛かりなものとなり易い。また、柵が頑丈であることから、ロボットが柵に衝突した場合には、ロボット自体が破損する可能性もある。このような事情から、上述したような頑丈で大掛かりな柵を用いずに安全にロボットを使用できる構成が望まれている。

ここで、ロボットの可動範囲内に人等の侵入を許す場合に上述したような柵を用いない方法としては、ロボットと人等との衝突を柵等によって物理的に防ぐのではなく、例えばいわゆるソフトウェアリミットのように制御によって動作領域をソフトウェア的に制限することが考えられる。この場合、例えばロボット制御装置は、ロボットの可動範囲内に動作領域を設定する。そして、ロボット制御装置は、ロボットが動作領域から出たこと、つまりロボットが指定領域内へ侵入したか否かを監視し、ロボットが指定領域内へ侵入したことを検知した場合に停止指令を発行してロボットを停止させる。

しかしながら、このような従来の制御方法では、信号伝達のタイムラグやモータが停止するまでの時間等によって、ロボット制御装置が停止指令を発行してからロボットが実際に停止するまでに時差が生じ、その時差によってロボットに滑走が生じる。そして、ロボットが高速で動作する場合、ロボットが指定領域に侵入したことをロボット制御装置が検知してから停止指令を発行してロボットの停止を開始させたのでは、ロボットの滑走によって指定領域内へ大きく侵入を許すことになってしまい、人等への安全が確保し難い。そのため、ロボットの滑走を考慮し、ロボットが指定領域に侵入することを事前に予測して、ロボットが指定領域に侵入する以前に停止指令を発行してロボットの停止を開始することが望ましい。

一方で、滑走による移動距離は、ロボットのモータの性能や個体差、及び劣化等の影響を受けるため、各ロボットで異なるだけでなく同じロボットでも使用期間によって変化することがある。このため、上述したように、ロボットが指定領域に侵入することを事前に予測し、ロボットが指定領域に侵入する以前に停止指令を発行してロボットの停止を開始する構成とする場合、安全性を担保するためには最悪条件を想定した余裕のある予測を行う必要がある。すると、実際にはまだロボットを停止させる必要がないにも関わらず、ロボットを停止させてしまう事態が頻発することになる。その結果、ロボットの性能が無駄に制限されてしまったり、ティーチングが難しくなってティーチング時間が延びてしまったりといった利便性が低下する事態が生じる。

特許第５３６５５２４号公報

本実施形態は上述した事情に鑑みてなされたものであり、その目的は、ロボットの性能を無駄に制限することなく、かつ、限られたスペースで安全にロボットを使用することができるロボット制御装置を提供することにある。

本実施形態のロボット制御装置は、多関節型のロボットを駆動制御するロボット制御装置である。ロボット制御装置は、前記ロボットの動作を指令する動作指令を受け取り前記動作指令に基づいて前記ロボットを動作させるとともに停止指令を受け取った場合に前記ロボットを緊急停止させる駆動部と、前記駆動部に前記動作指令を発行する制御処理を実行可能な制御処理部と、前記ロボットの現在の姿勢及び速度を算出する算出処理を実行可能な算出処理部と、前記ロボットの現在の姿勢及び速度に基づいて、前記ロボットの現在の位置で前記停止指令を発行した場合に前記ロボットが停止する位置である第１停止予測位置を予測する第１予測処理を実行可能な第１予測処理部と、前記第１停止予測位置よりも先の位置となるように調整された第２停止予測位置を予測する第２予測処理を実行可能な第２予測処理部と、予め設定された指定領域内に前記第１停止予測位置が侵入すると判断した場合に前記停止指令を前記駆動部に発行する第１監視処理と、前記指定領域内に前記第２停止予測位置が侵入すると判断した場合に前記ロボットの速度を現在の速度から減速させる減速指令を前記制御処理部に発行する第２監視処理と、を実行可能な監視処理部と、を備える。

これによれば、ロボット制御装置は、ロボットが動作している間、緊急停止させるための基準となる第１停止予測位置を監視するとともに、減速させるための基準となる第２停止予測位置を監視する。そして、第２停止予測位置は、第１停止予測位置に対して先の位置にとなるように設定されている。つまり、第２停止予測位置は、第１停止予測位置に対して、ロボットが停止するまでの距離に余裕を持って設定されている。そのため、第２停止予測位置は、第１停止予測位置に比べて、指定領域内への侵入が先に検知される。そして、ロボット制御装置は、第２停止予測位置が指定領域内に侵入すると判断した場合には、ロボットを停止させるのではなく減速させる。

このように、ロボットを減速させることで、緊急停止させた際の滑走による移動距離を短くすることができ、その結果、ロボットが指定領域に侵入する危険性を低下させることができる。これにより、第１停止予測位置までもが指定領域内に侵入して停止指令が発行されることを回避できる。その結果、ロボットの動作速度を無駄に制限させたり無駄に停止させたりすることを抑制でき、ロボットを動作させ続けることができる。一方で、ロボットを減速させたにも係わらず、万一、第１停止予測位置が指定領域内に侵入すると判断された場合には、停止信号を発行してロボットを緊急停止させる。これらの結果、ロボットの性能を無駄に制限することなく、かつ、限られた狭いスペースでも安全にロボットを使用することができる。

第１実施形態によるロボット制御装置の機能ブロックの一例を概念的に示すブロック図 第１実施形態によるロボット制御装置について、ロボットの可動範囲、指定領域、及び動作領域の一例を示す図 第１実施形態によるロボット制御装置について、第１停止予測位置及び第２停止予測位置の算出方法の一例を概念的に示す図 第１実施形態によるロボット制御装置について、通常制御部で実行される制御内容の一例を示すフローチャート 第１実施形態によるロボット制御装置について、安全監視部で実行される制御内容の一例を示すフローチャート 第１実施形態によるロボット制御装置について、第１停止予測位置及び第２停止予測位置の両方が指定領域に侵入しない場合を示すもので、（Ａ）は経過時間とロボットの位置との関係を示し、（Ｂ）は経過時間とロボットの速度との関係を示す図 第１実施形態によるロボット制御装置について、第２停止予測位置が指定領域に侵入しかつ第１停止予測位置が指定領域に侵入しない場合を示すもので、（Ａ）は経過時間とロボットの位置との関係を示し、（Ｂ）は経過時間とロボットの速度との関係を示す図 第１実施形態によるロボット制御装置について、第１停止予測位置及び第２停止予測位置の両方が指定領域に侵入した場合を示すもので、（Ａ）は経過時間とロボットの位置との関係を示し、（Ｂ）は経過時間とロボットの速度との関係を示す図 第２実施形態によるロボット制御装置の機能ブロックの一例を概念的に示すブロック図

以下、複数の実施形態について図面を参照しながら説明する。なお、各実施形態において、同一の構成には同一の符号を付して説明を省略する。

（第１実施形態）
以下では、第１実施形態について図１から図８を参照して説明する。図１に示すロボット制御装置２０は、例えば図２に示す多関節型のロボット１０の駆動を制御するためのものである。ロボット１０は、多関節すなわち多軸のアーム１１を有する垂直多関節型のロボットで構成することができ、ロボット制御装置２０によって制御される。なお、ロボット１０は、例えば水平多関節型のロボットやパラレルリンク側のロボット、直交型のロボット等で構成することもできる。

ロボット１０は、図２に示す複数の関節を有する多軸のアーム１１の他、図１に示すように、複数のモータ１２及び位置センサ１３を有している。モータ１２及び位置センサ１３の数は、アーム１１の関節数によって異なる。なお、図１では、説明を簡単にするため１つのモータ１２及び位置センサ１３のみを示している。各モータ１２は、アーム１１の各関節を動作させるためのものであり、ロボット制御装置２０によって駆動制御される。各位置センサ１３は、アーム１１の各関節の角度位置を検出するためのものであり、各モータ１２の図示しないモータ軸に取り付けられた例えばロータリーエンコーダで構成することができる。また、ロボット１０は、例えば手先にチャック等のツール９０を装着することができる。

ロボット制御装置２０は、図１に示すように、例えばＣＰＵ２０１及び記憶領域２０２を備えている。記憶領域２０２は、ロボット１０の駆動を制御するためのプログラムを記憶している。ロボット制御装置２０は、記憶領域２０２が記憶しているプログラムをＣＰＵ２０１において実行することにより、アーム１１が予め定められた所定の動作を自動的に実行するように各モータ１２を制御する。なお、ロボット制御装置２０は、例えばロボットコントローラと称されるロボット１０を制御することに特化した処理装置と、パソコン等の汎用的な処理装置とを組み合わせて構成することもできる。

ロボット制御装置２０は、例えば図示しないティーチングペンダントやパソコン、スマートフォン、若しくはタブレット端末等の外部装置を接続可能に構成されている。この場合、ロボット制御装置２０は、ロボット制御装置２０に接続される外部装置を、ロボット制御装置２０に対して各種設定内容等を入力するための入力装置や、ロボット制御装置２０から送信された各種情報を出力するための出力装置として機能させることができる。

ロボット制御装置２０は、図１に示すように、駆動部２１、設定処理部２２、算出処理部２３、安全監視部２４、及び通常制御部２５を備えている。駆動部２１は、例えばインバータ回路等の駆動回路を含んで構成されている。駆動部２１は、通常制御部２５で発行される動作指令を受け取り、その動作指令に基づいて各モータ１２を駆動させてロボット１０を動作させる機能を有する。動作指令は、ロボット１０の動作に関し、例えばロボット１０の特定部位の移動先の位置つまり目標位置や動作速度に関する指令を含んでいる。また、駆動部２１は、安全監視部２４で発行される停止指令を受け取ると、各モータ１２を停止させてロボット１０の動作を緊急停止させる機能を有する。

設定処理部２２は、設定処理を実行可能である。設定処理は、ロボット１０の動作に先立って予め指定領域Ａ１を設定し記憶領域２０２に記憶させる処理を含む。指定領域Ａ１は、図２に示すように、ロボット１０の特定部位の可動範囲Ａ０内であって、かつ、人等の侵入を許す領域すなわちロボット１０の侵入を禁止する範囲である。可動範囲Ａ０を決定するためのロボット１０の特定部位は、例えば人等に接触した場合に危険な任意の部位を設定することができる。例えばロボットの特定部位Ｒは、図２に示すように、アーム１１を最大まで伸ばした場合の最先端部に設定することができる。この場合、例えばロボット１０の手先にツールが装着されている場合、そのツール９０を含めて最先端の部位を特定部位Ｒとして設定することができる。すなわち、ロボット１０の可動範囲Ａ０は、ツール９０を含めて考慮することができる。

ここで、可動範囲Ａ０のうち指定領域Ａ１以外の領域、この場合、指定領域Ａ１の内側の領域は、ロボット１０の動作領域Ａ２となる。動作領域Ａ２は、ロボット１０の動作が許可された領域、すなわち人等の侵入が禁止された領域である。換言すれば、動作領域Ａ２は、人等の侵入を防ぐ必要がある領域である。この場合、設定処理は、例えば作業者が指定領域Ａ１を直接指定する構成でも良いし、例えば作業者が動作領域Ａ２を指定することで間接的に指定領域Ａ１が指定される構成でも良い。また、設定処理は、例えばロボット制御装置２０に接続された図示しない外部装置を作業者が操作することで、ロボット制御装置２０に対して設定情報を入力するように構成することができる。

なお、図２に示された境界線Ｐｌｉｍは、指定領域Ａ１と動作領域Ａ２との境界を示している。この場合、指定領域Ａ１と動作領域Ａ２との境界つまり境界線Ｐｌｉｍ上には、図２には示していないが、人等が動作領域Ａ２内に侵入することを防ぐための柵等を設けることが好ましい。この場合に設ける柵は、人等の侵入防止を主目的としており、ロボット１０が高速で衝突することがないため、衝突から人等を保護するための強度は要しない。

算出処理部２３は、ロボット１０の位置センサ１３からアーム１１の各関節の位置情報を受け取るとともに、算出処理を実行可能である。算出処理は、例えば位置センサ１３が検出したアーム１１の各関節の現在の角度位置から、ロボット１０の現在の姿勢や位置、及び現在の動作速度を算出する、つまりアーム１１の現在の姿勢や特定部位Ｒの位置、及び各関節の動作速度を算出する処理を含む。算出処理部２３は、算出処理によって算出したロボット１０の現在の姿勢や特定部位Ｒの位置及び動作速度を、安全監視部２４及び通常制御部２５に受け渡す。なお、算出処理部２３は、例えば安全監視部２４又は通常制御部２５に組み込まれていても良い。

安全監視部２４は、例えば記憶領域２０２に記憶されている動作プログラムを実行することにより、ロボット１０の動作を監視し、ロボット１０の動作中にロボット１０の特定部位Ｒが指定領域Ａ１内に侵入する可能性が生じた場合に、駆動部２１に対して停止指令を発行する機能を有する。駆動部２１は、安全監視部２４から停止指令を受け取ると、モータ１２を停止させてアーム１１の動作つまりロボット１０の動作を緊急停止させる。この場合、駆動部２１は、停止指令を最優先で実行する。すなわち、駆動部２１は、停止指令と同時に例えば動作指令を受け取った場合、動作指令よりも停止指令を優先して、ロボット１０を緊急停止させる。

本実施形態の場合、安全監視部２４は、第１予測処理部２４１、及び監視処理部としての第１監視処理部２４２を有している。この場合、安全監視部２４は、ＣＰＵ２０１とは異なる図示しないＣＰＵを有していても良い。そして、第１予測処理部２４１及び第１監視処理部２４２は、安全監視部２４が有する図示しないＣＰＵにおいて所定のプログラムを実行することでソフトウェアによって仮想的に実現しても良いし、集積回路等によってハードウェア的に実現しても良い。

第１予測処理部２４１は、算出処理部２３からロボット１０の現在の姿勢や特定部位Ｒの位置及び動作速度を受け取る。そして、第１予測処理部２４１は、第１予測処理を実行可能である。第１予測処理は、算出処理部２３で算出されたロボット１０の現在の姿勢や特定部位Ｒの位置及び動作速度に基づいて、図３等に示す第１停止予測位置Ｐｓ１を予測する処理を含む。

第１停止予測位置Ｐｓ１は、現在の位置つまり現時点で停止指令を発行した場合に、ロボット１０が完全に停止したときのロボット１０の特定部位Ｒの位置を予測したものである。つまり、第１停止予測位置Ｐｓ１は、空走時間と滑走時間とを考慮した位置である。本実施形態において、空走時間とは、停止指令が発行されてからモータ１２が停止のための動作を開始するまでの時間を意味する。また、滑走時間は、モータ１２が停止のための減速を開始してから完全に停止するまでの間の時間を意味する。

また、本実施形態において、空走とは、空走時間中にロボット１０が動作することを意味する。空走距離とは、空走によってロボット１０が移動した距離つまり空走時間中にロボット１０が移動した距離を意味する。また、滑走とは、滑走時間中にロボット１０が動作することを意味する。そして、滑走距離とは、滑走によってロボット１０が移動した距離つまり滑走時間中にロボット１０が移動した距離を意味する。

例えばロボット１０の特定部位Ｒをツール９０の先端部に設定した場合、第１停止予測位置Ｐｓ１は、ロボット１０の動作が完全に停止したときにツール９０の先端部が存在すると予測される位置となる。第１予測処理部２４１は、算出した第１停止予測位置Ｐｓ１を第１監視処理部２４２に受け渡す。

第１監視処理部２４２は、監視処理部を構成するものであり、第１監視処理を実行可能である。第１監視処理は、設定処理によって予め設定された指定領域Ａ１内に第１停止予測位置Ｐｓ１が侵入すると判断した場合に、停止指令を駆動部２１に発行する処理を含む。すなわち、第１監視処理部２４２は、ロボット１０が動作している間、常に第１停止予測位置Ｐｓ１を監視する。そして、第１監視処理部２４２は、指定領域Ａ１内に第１停止予測位置Ｐｓ１が侵入することを検知すると、駆動部２１に対して停止指令を発行する。

通常制御部２５は、ロボット１０の通常の動作つまりロボット１０の特定部位Ｒが指定領域Ａ１内に侵入していない場合における動作を制御する機能を有する。また、通常制御部２５は、ロボット１０の動作を監視し、ロボット１０の動作中にロボット１０の特定部位Ｒが指定領域Ａ１内に侵入する可能性が生じた場合に、安全監視部２４が停止指令を発行する前に、ロボット１０の動作速度つまりモータ１２の速度を減速させる機能を有する。

本実施形態の場合、通常制御部２５は、第２予測処理部２５１、監視処理部としての第２監視処理部２５２、及び制御処理部２５３を有している。第２予測処理部２５１、第２監視処理部２５２、及び制御処理部２５３は、ＣＰＵ２０１又は通常制御部２５が有する図示しないＣＰＵにおいて所定のプログラムを実行することでソフトウェアによって仮想的に実現しても良いし、集積回路等によってハードウェア的に実現しても良い。第２予測処理部２５１は、算出処理部２３からロボット１０の現在の姿勢や特定部位Ｒの位置及び動作速度を受け取る。そして、第２予測処理部２５１は、第２予測処理を実行可能である。第２予測処理は、算出処理部２３で算出されたロボット１０の現在の姿勢や特定部位Ｒの位置及び動作速度に基づいて、図３等に示す第２停止予測位置Ｐｓ２を予測する処理を含む。

第２停止予測位置Ｐｓ２は、第１停止予測位置Ｐｓ１と同様に、空走時間と滑走時間とを考慮した位置である。第２停止予測位置Ｐｓ２は、第１停止予測位置Ｐｓ１よりも先の位置となるようにパラメータが調整されたものである。第２停止予測位置Ｐｓ２は、現在の位置つまり現時点で停止指令を発行した場合に、ロボット１０が完全に停止したときのロボット１０の特定部位Ｒの位置を予測したものであって、かつ、第１停止予測位置Ｐｓ１よりも空走時間又は滑走時間のいずれか一方又は両方に余裕を持たせたものである。すなわち、第２停止予測位置Ｐｓ２は、第１停止予測位置Ｐｓ１よりも、より長い空走又は滑走を考慮したものである。第２予測処理部２５１は、算出した第２停止予測位置Ｐｓ２を第２監視処理部２５２に受け渡す。

第２監視処理部２５２は、監視処理部を構成するものであり、第２監視処理を実行可能である。第２監視処理は、設定処理によって予め設定された指定領域Ａ１内に第２停止予測位置Ｐｓ２が侵入すると判断した場合に、制御処理部２５３に対して減速指令を発行する。すなわち、第２監視処理部２５２は、ロボット１０が動作している間、常に第２停止予測位置Ｐｓ２を監視する。そして、第２監視処理部２５２は、指定領域Ａ１内に第２停止予測位置Ｐｓ２が侵入することを検知すると、制御処理部２５３に対して減速指令を発行する。減速指令は、ロボット１０の動作速度を現在の動作速度から減速させるための指令である。

制御処理部２５３は、制御処理を実行可能である。制御処理は、駆動部２１に動作指令を発行する処理を含む。制御処理部２５３は、減速指令を受け取っていない場合、通常の動作速度つまり予め設定された動作速度でロボット１０を動作させるための動作指令を発行する。一方、制御処理部２５３は、減速指令を受け取った場合、ロボット１０を現在の動作速度から減速させるための動作指令を発行する。この場合、駆動部２１は、ロボット１０を現在の動作速度から減速させるための動作指令を受け取ると、ロボット１０の動作速度を現在の動作速度から減速させる。

本実施形態の場合、安全監視部２４は機能安全に関する規格を満たしたものであり、一方で、通常制御部２５は機能安全に関する規格を満たしていないものである。すなわち、本実施形態の場合、第１予測処理部２４１及び第１監視処理部２４２は、機能安全に関する規格を満たしている。一方、第２予測処理部２５１及び第２監視処理部２５２は、機能安全に関する規格を満たさないものとすることができる。この場合、機能安全を満たしている安全監視部２４は、安全系と称することができ、機能安全を満たしていない通常制御部２５は、非安全系と称することができる。

安全監視部２４に求められる機能安全に関する規格とは、例えばＩＳＯ１０２１８等に規定されており、多重化若しくは多重安全の考えに基づくものである。安全監視部２４は、例えば複数系統の第１予測処理部２４１及び第１監視処理部２４２を有することで機能安全に関する規格を満たすことができる。これより、安全監視部２４は、１つの系統の第１予測処理部２４１及び第１監視処理部２４２が故障等で十分機能しなかった場合でも、他の系統が機能することにより、ロボット１０の監視及び停止指令を発行できるように構成されている。

次に、第１予測処理及び第２予測処理の一例つまり第１停止予測位置Ｐｓ１及び第２停止予測位置Ｐｓ２の算出方法の一例について説明する。図３に示すグラフは、横軸が時間ｔ、縦軸がロボット１０の特定部位Ｒの位置Ｐとし、時間ｔの経過に対するロボット１０の特定部位Ｒの位置Ｐの変化を概念的に示したものである。この場合、現時点ｔ０は現在の時点を示している。そして、現時点ｔ０の左側は過去つまり実測値を示しており、現時点ｔ０の右側は未来つまり予測値を示している。この場合、現在位置Ｐ０は、現時点ｔ０におけるロボット１０の特定部位Ｒの位置つまり算出処理部２３で算出された値となる。また、第１停止予測位置Ｐｓ１及び第２停止予測位置Ｐｓ２は、それぞれ第１予測処理部２４１及び第２予測処理部２５１で算出された予測値となる。

まず、第１予測処理について説明する。本実施形態において、第１予測処理は、現時点ｔ０から第１予測空走時間ｔｒ１の経過後にロボット１０の停止のための減速が開始するものとして第１停止予測位置Ｐｓ１を予測する処理を含んでいる。第１予測処理部２４１は、ロボット１０の現在位置Ｐ０、ロボット１０の現在速度Ｖ０、第１予測空走時間ｔｒ１、及び減速予測関数Ｓを用いて第１予測処理を実行することで、第１停止予測位置Ｐｓ１を算出することができる。ロボット１０の現在位置Ｐ０及び現在速度Ｖ０は、算出処理部２３で算出されたもの、つまり位置センサ１３の実測値に基づくものである。第１予測空走時間ｔｒ１は、第１停止予測位置Ｐｓ１を予測する際に用いる空走時間である。第１予測空走時間ｔｒ１は、停止指令を発行してからモータ１２が停止のための減速を開始するまでの時間差を想定したものであり、例えば反応時間やリアクションタイムと称することがある。

第１予測空走時間ｔｒ１は、例えばユーザ等によって予め設定され、また、ロボット１０の特性に合わせて適宜設定される。この場合、第１予測空走時間ｔｒ１は、ロボット１０を瞬時停止させた場合のモータ１２のリアクションタイム、つまりロボット１０の性能上の限界となる最短のリアクションタイムよりも長くなるように設定される。

減速予測関数Ｓは、モータ１２が停止のための減速を開始してから完全に停止するまでの期間のロボット１０の移動距離を算出するための関数であり、ロボット１０の位置と速度が入力値となる。減速予測関数Ｓは、ロボット１０の姿勢と速度を入力としたロボット１０の運動方程式を解いて緊急停止する期間中の加速度を求め、その加速度を積分することによって実現することができる。減速予測関数Ｓは、例えばロボット１０の関節数やアーム１１の長さ等によって適宜設定される。

第１予測処理部２４１は、まず、現在位置Ｐ０、第１予測空走時間ｔｒ１、及び現在速度Ｖ０を用いて、下記の式１に従って第１減速開始予測位置Ｐｒ１を求める。第１減速開始予測位置Ｐｒ１は、現時点ｔ０から第１予測空走時間ｔｒ１が経過した時点のロボット１０の位置を予測したものである。
Ｐｒ１＝Ｐ０＋ｔｒ１×Ｖ０…（式１）

次に、第１予測処理部２４１は、減速予測関数Ｓ、現在速度Ｖ０、及び上記式１で算出した第１減速開始予測位置Ｐｒ１を用いて、下記の式２に従って第１停止予測位置Ｐｓ１を算出する。なお、この場合、現時点ｔ０から第１予測空走時間ｔｒ１が経過するまではロボット１０は減速及び加速しないため、現時点ｔ０から第１予測空走時間ｔｒ１が経過した時点のロボット１０の速度は、現在速度Ｖ０に等しいものとして扱うことができる。
Ｐｓ１＝Ｐｒ１＋Ｓ（Ｐｒ１，Ｖ０）…（式２）

次に第２予測処理について説明する。本実施形態において、第２予測処理は、現時点ｔ０から、第１予測空走時間ｔｒ１よりも長い時間に設定された第２予測空走時間ｔｒ２の経過後にロボット１０の停止のための減速が開始するものとして第２停止予測位置Ｐｓ２を予測する処理を含んでいる。すなわち、第２予測処理部２５１は、ロボット１０の現在位置Ｐ０、ロボット１０の現在速度Ｖ０、第２予測空走時間ｔｒ２、及び減速予測関数Ｓを用いて第２予測処理を実行することで、第２停止予測位置Ｐｓ２を算出することができる。本実施形態の場合、第２予測処理は、第１予測処理で用いる減速予測関数Ｓと同一の減速予測関数Ｓを用いることができる。この場合、第１予測処理と第２予測処理とでは、減速予測関数Ｓに入力する減速開始予測位置Ｐｒ１、Ｐｒ２が異なるのみである。

第２予測空走時間ｔｒ２は、第２停止予測位置Ｐｓ２を予測する際に用いる空走時間である。第２予測空走時間ｔｒ２は、第１予測空走時間ｔｒ１と同様に、停止指令を発行してからモータ１２が停止のための減速を開始するまでの時間差を想定した時間であり、例えば反応時間やリアクションタイムと称することがある。第２予測空走時間ｔｒ２も、予め設定される。例えば設定処理部２２は、第１予測空走時間ｔｒ１の値に応じて自動で第２予測空走時間ｔｒ２を設定する。この場合、設定処理部２２は、第２予測空走時間ｔｒ２を第１予測空走時間ｔｒ１よりも長く設定する。例えば第２予測空走時間ｔｒ２は、第１予測空走時間ｔｒ１の２倍以上に設定される。これにより、第２停止予測位置Ｐｓ２が、第１停止予測位置Ｐｓ１よりも先の位置となるように調整される。

第２予測処理部２５１は、まず、現在位置Ｐ０、第２予測空走時間ｔｒ２、及び現在速度Ｖ０を用いて、下記の式３に従って第２減速開始予測位置Ｐｒ２を求める。第２減速開始予測位置Ｐｒ２は、現時点ｔ０から第２予測空走時間ｔｒ２が経過した時点のロボット１０の位置を示している。
Ｐｒ２＝Ｐ０＋ｔｒ２×Ｖ０…（式３）

次に、第２予測処理部２５１は、減速予測関数Ｓ、現在速度Ｖ０、及び上記式３で算出した第２減速開始予測位置Ｐｒ２を用いて、下記の式４に従って第２停止予測位置Ｐｓ２を算出する。なお、この場合、第１予測処理と同様に、現時点ｔ０から第２予測空走時間ｔｒ２が経過するまではロボット１０は減速及び加速しないため、現時点ｔ０から第２予測空走時間ｔｒ２が経過した時点のロボット１０の速度は、現在速度Ｖ０に等しいもとして扱うことができる。
Ｐｓ２＝Ｐｒ２＋Ｓ（Ｐｒ２，Ｖ０）…（式４）

次に、図４から図８も参照して、安全監視部２４及び通常制御部２５で実行される制御フローについて説明する。なお、図６から図８の（Ａ）に示すグラフは、図３に示すグラフと同様に、横軸が時間ｔ、縦軸がロボット１０の特定部位Ｒの位置Ｐとしており、時間ｔの経過に対するロボット１０の特定部位Ｒの位置Ｐの変化を概念的に示したものである。この場合、図６から図８の（Ａ）に示された境界線Ｐｌｉｍは、図２で示した指定領域Ａ１と動作領域Ａ２との境界線を示している。図６から図８の（Ａ）において、境界線Ｐｌｉｍの上側は指定領域Ａ１となり、境界線Ｐｌｉｍの下側は動作領域Ａ２となる。

また、図６から図８の（Ｂ）に示すグラフは、横軸が時間ｔ、縦軸がロボット１０の特定部位Ｒの動作速度Ｖとしており、時間ｔの経過に対するロボット１０の特定部位Ｒの動作速度Ｖの変化を概念的に示したものである。また、図７の（Ｂ）に破線で示す動作速度Ｖｘは、減速指令及び停止指令のいずれもが発行されなかった場合におけるロボット１０の特定部位Ｒの実際の速度を概念的に示したものである。

また、図７の（Ａ）に破線で示す位置Ｐｘは、減速指令の発行によってロボット１０の特定部位Ｒの実際の位置が変化する様子を概念的に示したものであり、図７の（Ｂ）に破線で示す動作速度Ｖｘは、減速指令の発行によってロボット１０の特定部位Ｒの実際の速度が変化する様子を概念的に示したものである。そして、図８の（Ａ）に破線で示す位置Ｐｘは、停止指令の発行によってロボット１０の特定部位Ｒの実際の位置が変化する様子を概念的に示したものであり、図８の（Ｂ）に破線で示す動作速度Ｖｘは、停止指令の発行によってロボット１０の特定部位Ｒの実際の速度が変化する様子を概念的に示したものである。

ロボット１０が動作している間、通常制御部２５は図４に示す処理を実行するとともに、安全監視部２４は図５に示す処理を実行する。この場合、図４に示す処理において、ステップＳ１２は第２停止位置予測処理に相当し、ステップＳ１３は第２監視処理に相当する。また、図５に示す処理において、ステップＳ２２は第１停止位置予測処理に相当し、ステップＳ２３は第１監視処理に相当する。

通常制御部２５は、ロボット１０の動作が開始すると、図４に示す処理を開始する（スタート）。そして、ステップＳ１１において、第２予測処理部２５１は、算出処理部２３で算出されたロボット１０の現在の姿勢や特定部位Ｒの位置Ｐ０及び現在速度Ｖ０を取得する。次に、ステップＳ１２において、第２予測処理部２５１は、第２予測処理を実行して、第２停止予測位置Ｐｓ２を算出する。その後、ステップＳ１３において、第２監視処理部２５２は、第２監視処理を実行して、第２停止予測位置Ｐｓ２が指定領域Ａ１内に侵入するか否かを判断する。

第２監視処理部２５２は、例えば図６に示すように第２停止予測位置Ｐｓ２が指定領域Ａ１内に侵入しないつまり境界線Ｐｌｉｍを超えないと判断した場合（図４のステップＳ１３でＮＯ）、ステップＳ１１へ処理を戻す。そして、通常制御部２５は、ステップＳ１１以降を再度実行する。この場合、ロボット１０の動作速度Ｖｘは、図６の（Ｂ）に示すように、減速されずに所定の速度が維持される。

一方、第２監視処理部２５２は、例えば図６に示すように第２停止予測位置Ｐｓ２が指定領域Ａ１内に侵入するつまり境界線Ｐｌｉｍを超えると判断した場合（ステップＳ１３でＹＥＳ）、ステップＳ１４へ処理を移行させる。そして、第２監視処理部２５２は、ステップＳ１４において、制御処理部２５３に対して減速指令を発行する。そして、減速指令を受けた制御処理部２５３は、駆動部２１に対して減速させるための動作指令を発行し、これにより、図７の（Ｂ）に示すようにロボット１０の動作速度Ｖｘが減速される。そして、通常制御部２５は、ステップＳ１１へ処理を戻し、ステップＳ１１以降を繰り返す。

第２監視処理部２５２は、第２停止予測位置Ｐｓ２が指定領域Ａ１内に侵入すると判断している間、減速指令を発行し続ける。これより、ロボット１０の特定部位Ｒが指定領域Ａ１内に侵入する危険性がある間、ロボット１０の動作速度Ｖｘが減速し続ける（ステップＳ１３でＹＥＳ）。そして、ロボット１０の動作速度Ｖｘが十分に減速されて第２停止予測位置Ｐｓ２が指定領域Ａ１内に侵入しなくなる、つまり図６の（Ａ）に示すようなグラフになると（図４のステップＳ１３でＮＯ）、第２監視処理部２５２は、減速指令の発行を停止し、その結果、図７（Ｂ）の減速終了時点ｔｅ以降に示すようにロボット１０の減速が終了する。

次に安全監視部２４における制御内容を説明する。安全監視部２４は、ロボット１０の動作が開始すると、図５に示す処理を開始する（スタート）。第１予測処理部２４１は、まずステップＳ２１において、算出処理部２３から、算出処理部２３で算出されたロボット１０の現在の姿勢や特定部位Ｒの現在位置Ｐ０及び現在速度Ｖ０を取得する。次に、第１予測処理部２４１は、ステップＳ２２において、第１予測処理を実行して、第１停止予測位置Ｐｓ１を算出する。その後、ステップＳ２３において、第１監視処理部２４２は、第１監視処理を実行して、第１停止予測位置Ｐｓ１が指定領域Ａ１内に侵入するか否かを判断する。

安全監視部２４は、例えば図６及び図７の（Ａ）に示すように、第１停止予測位置Ｐｓ１が指定領域Ａ１内に侵入しないつまり境界線Ｐｌｉｍを超えないと判断した場合（ステップＳ２３でＮＯ）、ステップＳ２１へ処理を戻し、ステップＳ２１以降を実行する。この場合、ロボット１０の動作速度Ｖｘは、制御処理部２５３から発行される動作指令の内容に従う。

一方、第１監視処理部２４２は、例えば図８に示すように第１停止予測位置Ｐｓ１が指定領域Ａ１内に侵入するつまり境界線Ｐｌｉｍを超えると判断した場合（図５のステップＳ２３でＹＥＳ）、ステップＳ２４へ処理を移行させる。そして、第１監視処理部２４２は、ステップＳ２４において、駆動部２１に対して停止指令を発行する。停止指令を受けた駆動部２１は、制御処理部２５３から受けた動作指令の内容に係わらず、モータ１２を停止させてロボット１０を緊急停止させる。そして、安全監視部２４は、詳細は図示しないが例えばユーザに対してエラーを報知する等し、その後、一連の制御を終了する（エンド）。

以上説明した実施形態によれば、ロボット制御装置２０は、多関節型のロボット１０を駆動制御するロボット制御装置である。ロボット制御装置２０は、駆動部２１と、制御処理部２５３と、算出処理部２３と、第１予測処理部２４１と、第１監視処理部２４２と、第２予測処理部２５１と、第２監視処理部２５２と、を備える。

駆動部２１は、ロボット１０の動作を指令する動作指令を受け取り動作指令に基づいてロボット１０を動作させるとともに停止指令を受け取った場合にロボット１０を緊急停止させる機能を有する。制御処理部２５３は、駆動部２１に動作指令を発行する制御処理を実行可能である。算出処理部２３は、算出処理を実行可能である。算出処理は、ロボット１０の現在の姿勢、位置Ｐ０、及び速度Ｖ０を算出する処理を含む。

第１予測処理部２４１は、第１予測処理を実行可能である。第１予測処理は、ロボット１０の現在の姿勢、位置Ｐ０、及び速度Ｖ０に基づいて、ロボット１０の現在の位置Ｐ０で停止指令を発行した場合にロボット１０が停止する位置である第１停止予測位置Ｐｓ１を予測する処理を含む。第１監視処理部２４２は、第１監視処理を実行可能である。第１監視処理は、予め設定された指定領域Ａ１内に第１停止予測位置Ｐｓ１が侵入すると判断した場合に停止指令を駆動部２１に発行する処理を含む。

第２予測処理部２５１は、第２予測処理を実行可能である。第２予測処理は、第１停止予測位置Ｐｓ１よりも先の位置となるように調整された第２停止予測位置Ｐｓ２を予測する処理を含む。そして、第２監視処理部２５２は、第２監視処理を実行可能である。第２監視処理は、指定領域Ａ１内に第２停止予測位置Ｐｓ２が侵入すると判断した場合にロボット１０の速度を現在の速度Ｖ０から減速させる減速指令を制御処理部２５３に発行する処理を含む。

これによれば、ロボット制御装置２０は、ロボット１０が動作している間、緊急停止させるための基準となる第１停止予測位置Ｐｓ１を監視するとともに、減速させるための基準となる第２停止予測位置Ｐｓ２を監視する。そして、第２停止予測位置Ｐｓ２は、第１停止予測位置Ｐｓ１に対して先の位置にとなるように設定されている。つまり、第２停止予測位置Ｐｓ２は、第１停止予測位置Ｐｓ１に対して、ロボット１０が停止するまでの距離この場合空走距離に余裕を持って設定されている。そのため、第２停止予測位置Ｐｓ２は、第１停止予測位置Ｐｓ１に比べて、指定領域Ａ１内への侵入が先に検知される。そして、ロボット制御装置２０は、第２停止予測位置Ｐｓ２が指定領域Ａ１内に侵入すると判断した場合には、ロボット１０を停止させるのではなく減速させる。

このように、ロボット１０を減速させることで、緊急停止させた際の滑走による移動距離を短くすることができ、その結果、ロボット１０が指定領域Ａ１に侵入する危険性を低下させることができる。これにより、第１停止予測位置Ｐｓ１までもが指定領域Ａ１内に侵入して停止指令が発行されることを回避でき、その結果、ロボット１０の動作速度を無駄に制限させたり無駄に停止させたりすることを抑制できる。一方で、ロボット１０を減速させたにも係わらず、万一、第１停止予測位置Ｐｓ１が指定領域Ａ１内に侵入すると判断された場合には、停止信号を発行してロボット１０を緊急停止させる。これらの結果、ロボット１０の性能を無駄に制限することなく、かつ、限られた狭いスペースでも安全にロボット１０を使用することができる。

ここで、ロボット１０が緊急停止した場合、例えば作業者等がロボット１０の周囲の安全を確認したうえでロボット１０の動作を復帰させる必要がある。このため、一旦ロボット１０が緊急停止してしまうと、ロボット１０の復帰に手間と時間がかかる。これに対し、本実施形態によれば、ロボット１０を減速させてしまうものの、緊急停止を回避して動作させ続けることができるため、緊急停止の復帰に伴う手間や時間を削減することができる。

また、本実施形態の場合、第１予測処理は、現時点ｔ０から第１予測空走時間ｔｒ１の経過後にロボット１０の停止のための減速が開始するものとして第１停止予測位置Ｐｓ１を予測する処理を含む。また、第２予測処理は、現時点ｔ０から、第１予測空走時間ｔｒ１よりも長い時間に設定された第２予測空走時間ｔｒ２の経過後にロボット１０の停止のための減速が開始するものとして第２停止予測位置Ｐｓ２を予測する処理を含む。

これによれば、例えば第１停止予測位置Ｐｓ１と第２停止予測位置Ｐｓ２との算出に、同一の減速予測関数Ｓを用いることができる。その結果、例えば第１停止予測位置Ｐｓ１と第２停止予測位置Ｐｓ２との算出するためのパラメータの大部分を共通化できるため、第１停止予測位置Ｐｓ１と第２停止予測位置Ｐｓ２との算出するための設定を簡単なものとすることができる。

ここで、安全監視部２４つまり第１予測処理部２４１及び第１監視処理部２４２は、ロボット１０の緊急停止を担うものであるため、機能安全に関する規格を満たす必要がある。この場合、第２予測処理部２５１及び第２監視処理部２５２つまり通常制御部２５の機能も、安全監視部２４に組み込んで機能安全に関する規格を満たすように構成することも考えられる。すなわち、安全監視部２４と通常制御部２５とを同一のハードウェアで構成することも考えられる。しかしながら、通常制御部２５の機能を安全監視部２４に組み込もうとすると、制御処理部２５３や駆動部２１も機能安全に関する規格を満たすようにしなければならず、構成が複雑となって実装コストも増大する。

これに対し、本実施形態では、第２予測処理部２５１及び第２監視処理部２５２は、ロボット１０の緊急停止を担うものではない。また、通常制御部２５は、第１停止予測位置Ｐｓ１が指定領域Ａ１への侵入することを安全監視部２４が検知するよりも先に、第２停止予測位置Ｐｓ２の指定領域Ａ１への侵入を検知することができる。そのため、第２予測処理部２５１及び第２監視処理部２５２は、機能安全に関する規格を満たす必要がなく非安全系となる通常制御部２５に第２予測処理部２５１及び第２監視処理部２５２を組み込むことができる。

このように、本実施形態によれば、ロボット制御装置２０は、監視処理部として、第１監視処理を実行可能な第１監視処理部２４２と、第２監視処理を実行可能な第２監視処理部２５２と、を有している。この場合、第１監視処理部２４２第２監視処理部２５２とは異なるハードウェアで構成されている。そして、第１予測処理部２４１及び第１監視処理部２４２は、機能安全に関する規格を満たすように構成されている。これに対し、第２予測処理部２５１及び第２監視処理部２５２は、機能安全に関する規格を満たさなくても良い。これによれば、第２予測処理部２５１及び第２監視処理部２５２を安全監視部２４に組み込む場合に比べて、構成を簡単にでき実装コストの増大を抑制することができる。

（第２実施形態）
第２実施形態について、図９を参照して説明する。本実施形態において、ロボット制御装置２０は、第２予測処理部２５１に換えて、補正処理部２５４を有している。補正処理部２５４は、第２予測処理を実行可能である。本実施形態の第２予測処理は、第１予測処理で予測された第１停止予測位置Ｐｓ１を補正することによって第２停止予測位置Ｐｓ２を予測する処理を含んでいる。補正処理部２５４は、第１予測処理で予測された第１停止予測位置Ｐｓ１を読み取り、その第１停止予測位置Ｐｓ１を補正することで第２停止予測位置Ｐｓ２を算出する。

すなわち、本実施形態では、減速予測関数Ｓの計算を安全監視部２４でのみ行い、補正処理部２５４つまり通常制御部２５では減速予測関数Ｓの計算を行わない。この場合、第２停止予測位置Ｐｓ２は、下記の式５により表すことができる。そして、式５に上述した式１～式３を代入すると、第２停止予測位置Ｐｓ２を下記の式６で表すことができる。この場合、第１予測空走時間ｔｒ１と第２予測空走時間ｔｒ２との差が十分に小さい、つまり第１減速開始予測位置Ｐｒ１と第２減速開始予測位置Ｐｒ２との差が大きくなければ、第２停止予測位置Ｐｓ２に大きな誤差は生じない。
Ｐｓ２＝Ｐｒ２＋Ｓ（Ｐｒ１，Ｖ０）…（式５）
Ｐｓ２＝Ｐｓ１＋Ｐｒ２－Ｐｒ１…（式６）

このように、本実施形態によれば、補正処理部２５４が、第１予測処理で予測された第１停止予測位置Ｐｓ１を読み取り、その第１停止予測位置Ｐｓ１を補正することで第２停止予測位置Ｐｓ２を算出することで、補正処理部２５４の計算量を低減することができる。すなわち、本実施形態によれば、第２停止予測位置Ｐｓ２の算出に伴う通常制御部２５の負荷の増大を抑制することができる。

（その他の実施形態）
なお、本発明は上記し且つ図面に記載した各実施形態に限定されるものではなく、その要旨を逸脱しない範囲で任意に変形、組み合わせ、あるいは拡張することができる。
上記各実施形態で示した数値などは例示であり、それに限定されるものではない。
また、上記各実施形態は、適宜組み合わせることができる。

例えば、安全監視部２４と通常制御部２５とは、異なるハードウェアで構成することが好ましいが、同一のハードウェアで構成することもできる。また、例えば上述した実施形態では、減速予測関数Ｓを用いて第１予測処理及び第２予測処理を行ったが、減速予測関数Ｓを用いずに第１予測処理及び第２予測処理を行うこともできる。減速予測関数Ｓを用いずに第１予測処理及び第２予測処理を行う方法としては、例えば最悪条件における加速度を線形予測する方法を採用することもできる。これによれば、減速予測関数Ｓを用いた場合に比べて、予測精度は下がるものの計算量を少なくすることができるため、処理の負荷を低減することができる。

本開示は、実施例に準拠して記述されたが、本開示は当該実施例や構造に限定されるものではないと理解される。本開示は、様々な変形例や均等範囲内の変形をも包含する。加えて、様々な組み合わせや形態、さらには、それらに一要素のみ、それ以上、あるいはそれ以下、を含む他の組み合わせや形態をも、本開示の範疇や思想範囲に入るものである。

１０…ロボット、２０…ロボット制御装置、２１…駆動部、２３…算出処理部、２４１…第１予測処理部、２４２…第１監視処理部、２５１…第２予測処理部、２５２…第２監視処理部、２５３…制御処理部、Ａ１…指定領域、Ｐｓ１…第１停止予測位置、Ｐｓ２…第２停止予測位置、Ｖ０…現在速度、ｔ０…現時点、ｔｒ１…第１予測空走時間、ｔｒ２…第２予測空走時間

Claims (4)

1. 多関節型のロボットを駆動制御するロボット制御装置であって、
   前記ロボットの動作を指令する動作指令を受け取り前記動作指令に基づいて前記ロボットを動作させるとともに停止指令を受け取った場合に前記ロボットを緊急停止させる駆動部と、
   前記駆動部に前記動作指令を発行する制御処理を実行可能な制御処理部と、
   前記ロボットの現在の姿勢、位置、及び速度を算出する算出処理を実行可能な算出処理部と、
   前記ロボットの現在の姿勢、位置、及び速度に基づいて、前記ロボットの現在の位置で前記停止指令を発行した場合に前記ロボットが停止する位置である第１停止予測位置を予測する第１予測処理を実行可能な第１予測処理部と、
   前記第１停止予測位置よりも先の位置となるように調整された第２停止予測位置を予測する第２予測処理を実行可能な第２予測処理部と、
   予め設定された指定領域内に前記第１停止予測位置が侵入すると判断した場合に前記停止指令を前記駆動部に発行する第１監視処理と、前記指定領域内に前記第２停止予測位置が侵入すると判断した場合に前記ロボットの速度を現在の速度から減速させる減速指令を前記制御処理部に発行する第２監視処理と、を実行可能な監視処理部と、
   を備えるロボット制御装置。
2. 前記第１予測処理は、現時点から第１予測空走時間の経過後に前記ロボットの停止のための減速が開始するものとして前記第１停止予測位置を予測する処理を含み、
   前記第２予測処理は、現時点から、前記第１予測空走時間よりも長い時間に設定された第２予測空走時間の経過後に前記ロボットの停止のための減速が開始するものとして前記第２停止予測位置を予測する処理を含む、
   請求項１に記載のロボット制御装置。
3. 前記監視処理部として、前記第１監視処理を実行可能な第１監視処理部と、前記第２監視処理を実行可能な第２監視処理部と、を有し、
   前記第１予測処理部及び前記第１監視処理部は、機能安全に関する規格を満たすように構成され、
   前記第２予測処理部及び前記第２監視処理部は、前記機能安全に関する規格を満たさないように構成されている、
   請求項１又は２に記載のロボット制御装置。
4. 前記第２予測処理は、前記第１予測処理で算出された前記第１停止予測位置を補正することによって前記第２停止予測位置を予測する処理を含む、
   請求項１から３のいずれか一項に記載のロボット制御装置。

Images