JP2015208834A - ワークを把持して搬送するロボットの安全監視装置 - Google Patents

Abstract

【課題】ロボットによるワークの持上げ動作とワークの載置動作とを安全に行う。【解決手段】ロボット（Ｒ）を監視する安全監視装置（ＲＣ）は、ワークパラメータを切替えるワークパラメータ切替部（２２）と、ワークパラメータを用いて外部環境からロボットに作用する外力を外力推定値として推定する外力推定部（２３）と、外力推定値が外力判定条件を満たすときにロボットを停止させる外力監視部（２６）と、ロボットの位置が所定の領域内に含まれているか等に基づいてロボットを停止させる動作監視部（２７）と、動作監視部の無効指令と有効指令とを切替える動作監視状態切替部（２４）と、無効指令時には外力判定条件を動作監視無効時外力判定条件に切替えると共に、有効指令時には外力判定条件を動作監視有効時外力判定条件に切替える外力判定条件設定部（２５）とを含む。【選択図】図１

Description

本発明は、ワークを把持して搬送するロボットの安全監視装置に関する。さらに、本発明は、ロボットに作用する力またはトルクを検出する力検出部を備えたロボットを監視する安全監視装置に関する。

ロボットに取付けられた力検出部により、ロボットが外部環境から受ける力やトルク（以下、適宜「外力」と称する）を検出し、検出された外力に基づいてロボットを停止させたり、回避動作をさせたりしている。これにより、ロボットが外部環境に与える力を軽減することができる。

そのようなロボットがワークを搬送する際に、力検出部の検出値は、ロボットがワークを把持しているか否かに応じて変化する。つまり、検出値はワーク質量の分だけ変化しうる。従って、外力を正確に検出するためには、外力計算に使用されるワークの質量、重心位置および慣性行列（以下、ワークパラメータ）をワークに応じて切替える必要がある。

また、ロボットがワークを把持している場合であっても、力検出部がワークの質量全体を検出できない場合もありうる。例えば、ロボットが台上に載置されたワークを持上げる場合である。ワークは台によって支持されているので、ロボットがワークを単に把持しただけでは、力検出部はワークの質量を全く検出しないか、または質量の一部のみを検出する。そして、ロボットがワークを持上げるとワークが台から離間するので、力検出部は、ワークの質量全体を検出するようになる。従って、ロボットがワークを把持した後で且つワークが台から離間する前においては、力検出部は外力を正確に検出できない。

また、ロボットがワークを台に載置するときも同様である。すなわち、ロボットがワークを載置する際にワークが台に接触した後で且つロボットがワークを解放する前においては、力検出部は外力を正確に検出できない。従って、ロボットがワークを持上げる動作およびワークを載置する動作については、外力を監視したとしてもロボットの安全性を監視するのは困難である。

特許文献１は、工具交換時に安全性を監視するロボット安全監視装置を開示している。特許文献１においては、プログラム上の制御情報である現在の工具情報と現実に取付けられている工具の工具情報とが一致しない場合には、ロボットの位置が所定の領域内にあるか否かを監視することで、工具交換時の安全性を監視している。

また、特許文献２は、ワークを把持または解放する際にロボットの静止状態を維持する技術を開示している。この技術においては、ワークを把持したときに、ワークパラメータを切替えることによってロボットが動出すことを防止している。

特許第５０８８１５６号公報 特許第５１５４７１２号公報

ここで、ワークの把持に関して、特許文献１と同様の監視を行うことを考える。言い換えれば、外力計算で現在使用しているワークパラメータと、現実に把持しているワークのワークパラメータとが一致しない場合には、ロボットの位置が所定の領域内にあるか否かを監視する。しかしながら、この場合には、前述したように、ロボットがワークを持上げる動作およびワークを載置する動作においては、外力計算に使用しているワークパラメータと現実に把持しているワークパラメータとが一致しているにもかかわらず、外力を正しく検出できない場合がある。従って、特許文献１の手法を採用することは、安全性に問題がある。

また、特許文献２においては、ロボットが把持したワークを持上げる動作を行ったときの安全性に対しては対策されていないという問題がある。

本発明はこのような事情に鑑みてなされたものであり、ロボットがワークを持上げる動作およびロボットがワークを載置する動作を安全に行うことのできるロボットの安全監視装置を提供することを目的とする。

前述した目的を達成するために１番目の発明によれば、ロボットに作用する外力を検出する力検出部を備えたロボットを監視する安全監視装置において、前記ロボットに把持されるべきワークの質量と質量中心と慣性行列のうち少なくとも一つを含むワークパラメータを切替えるワークパラメータ切替部と、該ワークパラメータ切替部により切替えられたワークパラメータを用いて、外部環境から前記ロボットに作用する力またはトルクを外力推定値として推定する外力推定部と、前記外力推定値が外力判定条件を満たすときに前記ロボットを停止させる外力監視部と、前記ロボットの位置が所定の領域内に含まれているか、前記ロボットの移動方向が所定の方向か、ロボットの動作速度が所定の速度以下であるか、前記ロボットの動作加速度が所定の加速度以下であるかのうちの少なくとも一つの条件が満たされない場合に前記ロボットを停止させる動作監視部と、該動作監視部を無効にする動作監視無効指令と前記動作監視部を有効にする動作監視有効指令とを切替える動作監視状態切替部と、前記動作監視無効指令に切替えられたときには前記外力判定条件を動作監視無効時外力判定条件に切替えると共に、前記動作監視有効指令に切替えられたときには前記外力判定条件を動作監視有効時外力判定条件に切替える外力判定条件設定部と、を具備する、ロボットの安全監視装置が提供される。
２番目の発明によれば、１番目の発明において、前記外力判定条件は、前記外力推定値が所定の第一値を超えたとき、前記外力推定値の移動平均が所定の第二値を超えたとき、所定時間前の前記外力推定値と現在の前記外力推定値との間の変位量が所定の第三値を超えたときの少なくとも一つの条件を含む。
３番目の発明によれば、１番目または２番目の発明において、前記動作監視有効指令に切替えられたときには、前記外力監視部を無効にするようにした。
４番目の発明によれば、１番目または２番目の発明において、前記動作監視無効時外力判定条件は前記外力推定値が第一閾値を超えたときを条件としており、前記動作監視有効時外力判定条件は前記外力推定値が前記第一閾値以上の第二閾値を超えたときを条件としている。
５番目の発明によれば、１番目から４番目のいずれかの発明において、前記動作監視状態切替部が前記動作監視有効指令から前記動作監視無効指令に切替えるときに、前記外力推定部により推定される前記外力推定値が所定条件を満たさない場合には、前記動作監視有効指令から前記動作監視無効指令への切替えを中止する。

１番目の発明においては、ロボットがワークを持上げる動作およびロボットがワークを載置する動作を行う際には、動作監視有効指令に切替えて、動作監視部が有効になるように外力判定条件を変更する。このため、外力監視部が、ロボットに外力がかかっていないにもかかわらず外力がかかったと判断してロボットを停止させないようにできる。従って、ロボットがワークを持上げる動作およびロボットがワークを載置する動作を安全に行うことができる。
２番目の発明においては、外力監視部がロボットを停止させる条件を適切に設定することができる。
３番目の発明においては、外力監視部を無効にしたとしても、ロボットの安全性を監視し続ける。このことは、台に形成された凹部に部分的に嵌合したワークをロボットが把持する場合に有利である。
動作監視部が有効である場合、例えばロボットがワークを持上げる動作およびワークを載置する動作においては、外力推定値が正確でない可能性がある。４番目の発明においては、動作監視有効時外力判定条件を動作監視無効時外力判定条件よりも緩くしている。このため、ロボットに外力が作用していないにもかかわらず、外力監視部がロボットに外力が作用したと誤判定するのを避けられる。
５番目の発明においては、外力推定値が所定条件を満たすか否かを通じて、外力監視部が正確に機能するか否かを確認している。そして、外力推定部により推定される外力推定値が所定条件を満たさない場合には、外力監視部が正確に機能しないと判断して、動作監視無効指令への切替えを中止する。従って、外力監視部が正確に機能しない場合には、動作監視部を有効にしてロボットの安全性を監視し続ける。

本発明における安全監視装置の基本構成を示す図である。 ロボットの状態と外力との関係を示す図である。 ロボットがワークを持上げる動作を行う際の安全監視装置の動作を示すフローチャートである。 ロボットがワークを載置する動作を行う際の安全監視装置の動作を示すフローチャートである。本発明に基づく安全監視装置の動作を示すフローチャートである。 或る実施形態においてロボットがワークを把持する前の状態を示す図である。 他の実施形態においてロボットがワークを把持する前の状態を示す図である。

以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
図１は本発明における安全監視装置の基本構成を示す図である。図１に示されるように、ロボットＲ、例えば六軸多関節ロボットはエンドエフェクタＥ、例えば把持部をその先端に備えている。図示されるように、ワークＷが台Ｔ上に載置されている。そして、力もしくはトルクを検出する力センサＳがロボットＲの基台部に取付けられている。ただし、力センサＳはロボットＲのロボットアームに内蔵されていてもよい。この場合には、力センサＳが、力センサＳが取付けられたロボットの一部分からロボットの先端およびワークまでに作用する力またはトルクを検出する。

また、ロボットＲの安全監視装置として機能するロボット制御装置ＲＣは、ロボットの動作プログラム２１を含んでいる。ロボット制御装置ＲＣのワークパラメータ切替部２２は、ワークＷの質量とワークＷの質量中心と慣性行列とのうちの少なくとも一つを含むワークパラメータを取得し、ワークＷに応じてワークパラメータを切替える。図１においては、ワークパラメータ切替部２２はワークパラメータを動作プログラム２１から取得している。ただし、図示しない入力部を用いて操作者がワークパラメータを入力してもよい。なお、ワークＷを把持していない状態においては、ワークパラメータの設定は、質量０、質量中心０、慣性行列０という形式で表現できる。ワークパラメータはワークＷに応じて異なる。従いまして、ワークパラメータはワークＷの種類などに応じて複数存在している。

ロボット制御装置ＲＣの外力推定部２３は、周辺機器などの外部環境からロボットＲに作用する力またはトルクを外力推定値として算出する。具体的には、外力推定部２３は力センサＳが検出した力またはトルクから、外力が作用していないときにおけるロボットＲおよびワークＷのそれぞれの自重と、ロボットＲとワークＷのそれぞれが移動することによって生じる慣性力によって力センサＳに作用する力またはトルクとを減算して、外力推定値を算出する。

ロボット制御装置ＲＣの動作監視状態切替部２４は、後述する動作監視部２７を有効にする動作監視有効指令と、動作監視部２７を無効にする動作監視無効指令とを切替える。図１においては、動作監視状態切替部２４は動作監視有効指令または動作監視無効指令を動作プログラム２１から取得している。ただし、図示しない入力部を用いて操作者が動作監視有効指令または動作監視無効指令を入力してもよい。一つの例においては、動作プログラム２１のロボットがワークを持上げる動作およびロボットがワークを載置する動作に、動作監視有効指令が記述されているものとする。

ロボット制御装置ＲＣの外力判定条件設定部２５は、動作監視状態切替部２４により切替られた動作監視状態に応じて外力判定条件を切替える。つまり、外力判定条件設定部２５は、動作監視無効指令に切替えられたときには外力判定条件を動作監視無効時外力判定条件に切替えると共に、動作監視有効指令に切替えられたときには外力判定条件を動作監視有効時外力判定条件に切替える。外力判定条件は、外力推定値を使用して、ロボットに外力が加わったかどうかを判定するための条件である。なお、動作監視有効時外力判定条件と動作監視無効時外力判定条件とは別の条件でもよいし、同じ条件でもよい。

ロボット制御装置ＲＣの外力監視部２６は、外力推定値が外力判定条件（動作監視有効時外力判定条件または動作監視無効時外力判定条件）を満たすときにロボットを停止させるよう動作する。

さらに、ロボット制御装置ＲＣの動作監視部２７は、ロボットＲの位置が所定の領域内に含まれているか、ロボットＲの移動方向が所定の方向であるか、ロボットＲの動作速度が所定の速度以下であるか、ロボットＲの動作加速度が所定の加速度以下であるかのうちの少なくとも一つの条件を評価する。そして、少なくとも一つの条件が成立していない場合には、動作監視部２７はロボットＲを停止させる。

つまり、本発明においては、外力監視部２６が外力推定値を通じてロボットの安全性を監視し、また、動作監視部２７もロボットの位置などを通じてロボットの安全性を監視する。

そして、動作監視無効指令に切替えられたときには外力監視部２６のみがロボットの安全性を監視する。動作監視有効指令に切替えられたときには、動作監視部２７がロボットの安全性を支配的に監視する。動作監視有効指令に切替えられたときには、外力監視部２６が監視を継続してもよく、後述するように外力監視部２６を無効にしてもよい。

なお、ロボットＲの位置が所定の領域内に含まれているかの条件は、ロボットＲの関節角度で判定してもよいし、ロボット先端部の位置で判定してもよい。ロボットＲの移動方向が所定の方向であることの判定は、ロボットＲの関節動作方向で判定してもよいし、ロボットＲの先端位置の動作方向で判定してもよい。ロボットＲの動作速度が所定の速度以下であることの判定は、ロボットＲの関節速度で判定してもよいし、ロボットＲの先端位置の速度で判定してもよい。ロボットＲの動作加速度が所定の加速度以下であることの判定は、ロボットの関節加速度で判定してもよいし、ロボットＲの先端位置の加速度で判定してもよい。

ここで、ワークＷによってロボットＲにかかる力やトルク（外力）について説明する。ここでは、図１に示されるように、力センサＳはロボットＲの基台部に取付けられているものとする。また、ロボットＲの質量は２００ｋｇで、ワークＷの質量は２０ｋｇとする。外力推定部２３は、以下の式（１）から外力を算出する。
（外力推定値）＝（力センサＳの検出値）−（ロボットＲおよびワークＷの質量によって力センサＳに作用する力の推定値）−（ロボットＲ自体の動作によって力センサＳに作用する慣性力の推定値） （１）

以下においてはロボットＲが静止している場合の外力推定値の鉛直方向成分について説明する。ロボットＲが静止している場合には上式の第三項は０である。そして、ロボットＲがワークＷを把持している場合には、第二項におけるワーク質量を考慮する。ロボットＲがワークＷを把持していない場合は、第二項におけるワーク質量を考慮しない。外力を正しく検出するためには、ロボットＲに外力が加わっていない場合の外力推定値は０ｋｇである必要がある。

図２はロボットの状態と外力との関係を示す図である。図２の左方には、ロボットＲがワークＷを把持する前の状態が示されている。この場合には、当然のことながら、ワーク質量は力センサＳに作用しない。従って、力センサＳの出力値は２００ｋｇである。外力計算においても、ワーク質量を含める必要はないので、外力計算に使う質量合計は２００ｋｇである。このとき、ロボットＲに外力が加わっていないときの外力推定値は０ｋｇである。この場合には、外力を正確に検出できる。

図２の中央には、ロボットＲがワークＷを把持したものの、ワークＷが台Ｔによって少なくとも部分的に支持されている状態が示されている。この場合には、把持力の方向に応じてワーク質量の一部が力センサＳに作用する。従って、力センサＳの検出値は（２００＋Ａ）ｋｇ（０≦Ａ＜２０ｋｇ）となる。そして、この場合には、ワークＷを把持しているので外力計算にワーク質量が考慮され、外力計算に使う質量合計は２２０ｋｇとなる。それゆえ、ロボットＲに外力が加わっていないときの外力推定値は（２０−Ａ）ｋｇとなる。従って、この場合には、外力を正確に検出できない。

図２の右方には、ロボットＲが把持されたワークＷを持上げた状態が示されている。この場合にはワークＷは台Ｔから完全に離間しているので、ワーク質量全体が力センサＳに作用する。従って、力センサＳの検出値は２２０ｋｇとなる。ワークＷを把持しているので外力計算にワーク質量が考慮され、外力計算に使う質量合計は２２０ｋｇである。そして、ロボットＲに外力が加わっていないときの外力推定値は０ｋｇとなる。従って、この場合には、外力を正確に検出できる。

それゆえ、ロボットＲがワークＷを把持した後で且つワークＷを持上げる前、つまりロボットＲがワークＷを持上げる動作においては、ワーク質量が力センサＳに作用する力を推定するのは困難である。ロボットがワークを載置する動作においても同様の問題がある。つまり、ロボットがワークを持上げる動作およびワークを載置する動作においては、外力を正確に検出できない。なお、力センサＳの代わりに、ロボットＲがトルクセンサ（図示しない）を備えている場合も同様である。

図３Ａはロボットがワークを持上げる動作を行う際の安全監視装置の動作を示すフローチャートである。この動作を行うにあたり、動作プログラム２１が読込まれるものとする。また、ワークＷは台Ｔ上に予め載置されているものとする。なお、図３Ａに示される動作を、図示しない入力部を用いて操作者が直接指令したり、外部の制御コントローラ（図示しない）で制御したりしてもよい。

はじめに、図３ＡのステップＳ１１において、ワークＷを把持できる位置までロボットＲを移動させる。次いで、ステップＳ１２において、動作監視状態切替部２４が、動作監視部２７を有効にする動作監視有効指令を出力する。

その後、ステップＳ１３において、ロボットＲがエンドエフェクタＥによりワークＷを把持する。次いで、ステップＳ１４において、ワークパラメータ切替部２２が、把持されたワークＷのワークパラメータを動作プログラム２１から取得する。

次いで、ステップＳ１５において、ロボットＲがワークＷを持上げる動作を行う。ワークＷが台Ｔから完全に離間するまでワークＷを持上げると、ステップＳ１６において、動作監視状態切替部２４が、動作監視部２７を無効にする動作監視無効指令を出力する。最終的に、ステップＳ１７において、ロボットＲがワークＷを所定の場所まで搬送する搬送動作を行う。

ところで、図３Ｂはロボットがワークを載置する動作を行う際の安全監視装置の動作を示すフローチャートである。このときには、ロボットＲはワークＷをエンドエフェクタＥにより既に把持しているものとする。そして、ステップＳ２１において、ロボットＲがワークＷを載置する位置まで移動する。次いで、ステップＳ２２において、動作監視状態切替部２４が、動作監視部２７を有効にする動作監視有効指令を出力する。次いで、ステップＳ２３において、ワークパラメータ切替部２２が、把持されたワークＷのワークパラメータを動作プログラム２１から取得する。

その後、ステップＳ２４においてロボットＲがワークＷを台Ｔ上に載置する動作を行う。次いで、ステップＳ２５においてロボットＲのエンドエフェクタＥを解放して、ワークＷを台Ｔに載置完了させる。次いで、動作監視状態切替部２４が、動作監視部２７を無効にする動作監視無効指令を出力する。最終的に、ステップＳ２７において、ロボットＲが元位置まで移動する。

このように本発明においては、ロボットＲがワークＷを持上げる動作およびワークＷを載置する動作において、動作監視有効指令が出力される。このときには、動作監視部２７がロボットＲの安全性を監視する。なお、動作監視有効指令が出力されないとき（動作監視無効指令が出力されているとき）には、外力監視部２６がロボットの安全性を監視するものとする。

図２を参照して説明したように、ロボットＲがワークＷを把持した後で且つワークＷを持上げる前においては正確な外力を得ることができない。また、ロボットＲがワークＷを載置する動作の際にも同様である。従って、これら期間においては、外力に基づく監視は無意味である。このため、本発明においては、ロボットＲがワークＷを持上げる動作およびワークＷを載置する動作の前において、動作監視有効指令が出力される。このときには、動作監視部２７がロボットＲの安全性を監視する。なお、動作監視有効指令が出力されないとき（動作監視無効指令が出力されているとき）には、外力監視部２６がロボットの安全性を監視するものとする。

図４は或る実施形態においてロボットがワークを把持する前の状態を示す図であり、図２の左方に示されるのと同様な図である。以下、図４を参照しつつ、ロボットＲが２０ｋｇのワークＷを持上げる場合の一例を考える。図４に示される座標系に関し、外力推定値のＸＹＺ三方向の成分をそれぞれｆｘ、ｆｙ、ｆｚとする。

また、動作監視無効時外力判定条件を、「｜ｆｘ｜＞３ｋｇ、｜ｆｙ｜＞３ｋｇまたは｜ｆｚ｜＞３ｋｇ」とする。そして、動作監視有効時外力判定条件を、「｜ｆｘ｜＞３ｋｇ、｜ｆｙ｜＞３ｋｇ、ｆｚ＜−３ｋｇまたはｆｚ＞＋２１ｋｇ」とする。

そして、図３ＡのステップＳ１２において動作監視状態が有効になると、外力判定条件設定部２５が外力判定条件を動作監視有効時外力判定条件に切替える。動作監視有効時外力判定条件は、把持されているワークＷが台Ｔに接触していて、台ＴからワークＴを通じてロボットＲに作用する力以外にはロボットＲに外力が加わっていない状態において、外力監視部２６が外力が加わったと検出しないような判定条件である。

そして、ワークＷを持上げるときに台ＴからワークＷを通じてロボットＲに加わる力が（２０−Ａ）ｋｇ（０≦Ａ＜２０ｋｇ）だけ＋Ｚ方向に作用していて、それ以外の外力が加わっていない状態では、外力推定値はｆｘ＝０ｋｇ、ｆｙ＝０ｋｇ、ｆｚ＝２０−Ａｋｇ（０≦Ａ＜２０ｋｇ）となる。動作監視有効時外力判定条件によれば、外力監視部２６はロボットＲに外力が加わったと誤判定しない。

この状態においては、＋Ｚ方向に２１ｋｇの外力が加われば、ｆｚ＝４１−Ａｋｇ（０≦Ａ＜２０ｋｇ）となる。つまり、ｆｚ＞＋２１ｋｇであるので、ロボットＲに外力が加わったと判定される。

そして、−Ｚ方向に２３ｋｇの外力が加われば、ｆｚ＝−３−Ａｋｇ（０≦Ａ＜２０ｋｇ）となる。つまり、ｆｚ＜−３ｋｇであるので、ロボットＲに外力が加わったと判定される。従って、これよりも小さい外力が加わった場合であっても、外力が加わったと判定されない。なお、ＸＹ方向の外力に対しては、動作監視状態に関わらず、同じ外力に対して、外力が加わったと判定する。

このように、本発明では、外力監視が正しく機能しないとき（ロボットがワークを持上げる動作およびワークを載置する動作）に、実際に外力が加わっていないにも関わらず、外力が加わったと判定しないように条件を変更する。従って、本発明では、外力監視部２６が誤判定してロボットＲを停止させることはなく、ロボットＲの動作効率が下がるのを防止できる。ただし、条件を変更している間は、外力監視部２６が実質的に機能しないので、動作監視部２７によって安全性を監視する。

動作監視状態が有効である場合には、動作監視部２７は、ロボットＲの位置が所定の領域内に含まれているか、ロボットＲの移動方向が所定の方向か、ロボットＲの動作速度が所定の速度以下であるか、ロボットＲの動作加速度が所定の加速度以下であるかの少なくとも一つの条件を評価する。この場合、すべての条件を評価しても良いし、評価しない条件があっても構わない。少なくとも一つの条件が成立していない場合には、動作監視部２７がロボットＲを停止させる。

ここで、ロボットＲがワークＷを把持した後で、ワークＷを持上げてワークＷが完全に台Ｔから離間した状態になるまでのロボットＲの動作領域が、所定の領域として予め設定されているものとする。なお、所定の領域として、ロボットＲの関節の角度の領域を設定してもよく、また、ロボットＲの先端部の動作領域を設定してもよい。

また、所定の方向として、ロボットＲがワークＷを持上げるときの動作方向を予め設定する。所定の方向として、各関節の動作方向を設定してもよく、ロボットＲの先端部の動作方向を設定してもよい。

さらに、所定の速度および所定の加速度として、仮にロボットＲが外部環境に接触している場合であっても、外部環境に与える力が許容できる力以下になるための速度および加速度を予め設定する。所定の速度および所定の加速度として、各関節の速度および加速度を設定してもよく、ロボットＲの先端部の速度および加速度を設定してもよい。

動作監視部２７がこのような動作監視を行うことにより、ロボットＲがワークＷを持上げるときにおける、外部環境に対するロボットＲの安全性を確保することができる。

なお、動作監視有効時外力判定条件を満たす外力推定値の集合は、動作監視無効時外力判定条件を満たす外力推定値の集合に、必ずしも含まれていなくてもよい。例えば、動作監視部２７がロボットＲの先端部の動作方向を監視する場合は、その動作方向以外の外力推定値に対する外力判定条件を厳しくしてもよい。

ロボットＲが加速または減速するときには、その動作方向にロボットＲの慣性力が生じる。本発明においては、慣性力を考慮して外力を推定する。しかしながら、ワークＷの質量には、個体差（バラツキ）があるので、慣性力を正確に推定するのは困難である。従って、ロボットＲが動作する方向に対しては外力推定値もバラつく可能性がある。

例えば、ワークパラメータにおけるワーク質量をｍ、個体差によるワーク質量のバラつきをΔｍとすると、実際のワーク質量は、ｍ＋Δｍと表される。そして、或る加速度α＝（ａｘ、ａｙ、ａｚ）でワークＷが移動するときには、移動することによって慣性力（ｍ＋Δｍ）αが生じる。このときワークパラメータにおけるワーク質量はｍであるので、推定できる慣性力はｍαである。前述した式（１）によれば、ワークＷの質量のバラつきによって外力推定値にΔｍαの誤差が生じることになる。

従って、ロボットＲ自体の慣性力のバラつきによって、実際にはロボットＲに外力が加わっていないにも関わらず、外力監視部２６がロボットＲに外力が加わったと誤判定することを防ぐ必要がある。そのためには、予め予測される外力推定値のバラつきの分だけ、外力判定条件の閾値を緩和するのが好ましい。動作監視状態が無効であるときには、ロボットＲは任意の方向に動作し、ワークＷも任意の方向に動作する可能性がある。このため、どの方向の外力判定条件についてもバラつき分を考慮する必要がある。

動作監視部２７においてロボットＲが動作する方向を監視する場合、動作監視状態が有効のときには、ロボットＲが実際に動く方向を限定することができる。言い換えれば、ロボットＲが動かない方向についての外力判定条件を、動作監視状態が無効のときよりも厳しくすればよい。

図４に示される実施例においては、ロボットＲはワークＷを＋Ｚ方向に持上げる。従って、動作監視条件にロボットＲの先端部が＋Ｚ方向にのみ動作することを設ける。つまり、ロボットＲが実際にＸＹ方向に動作しようとすると、ロボットＲが停止するようにする。この場合には、ＸＹ方向にワークＷの加速度が生じることがないので、ワーク質量のバラつきにより生じるＸＹ方向の外力推定値の誤差Δｍ×ａｘ、Δｍ×ａｙはゼロとなる。

Δｍ×ａ、Δｍ×ａｙの大きさをせいぜい１ｋｇと見積もる場合には、動作監視無効時外力判定条件を、「｜ｆｘ｜＞３ｋｇ、｜ｆｙ｜＞３ｋｇまたは｜ｆｚ｜＞３ｋｇ」とする。そして、動作監視有効時外力判定条件を、「｜ｆｘ｜＞２ｋｇ、｜ｆｙ｜＞２ｋｇ、ｆｚ＜−３ｋｇまたはｆｚ＞＋２１ｋｇ」とする。

そして、実際に＋Ｘ方向に２．５ｋｇの外力がロボットＲに加わった場合、動作監視無効時にはロボットＲに外力が加わったと判定されない。しかしながら、動作監視有効時には外力として判定される。

以上のように、動作監視有効である場合のほうが、慣性力のバラつきの影響が小さい。より小さい外力に対してロボットＲに外力が加わったと判定できる場合には、そのように外力判定条件を設定してもよい。

なお、動作監視無効時外力判定条件の閾値が十分に大きい場合には、動作監視無効時外力判定条件と動作監視有効時外力判定条件とが同じ条件であっても構わない。例えば前述した例において、動作監視無効時外力判定条件と動作監視有効時外力判定条件とを「｜ｆｘ｜＞３０ｋｇ、｜ｆｙ｜＞３０ｋｇまたは｜ｆｚ｜＞３０ｋｇ」としたとする。この条件では３０ｋｇより大きな外力が加わったときのみ、ロボットＲを停止させる。システム構成上、ロボットＲに３０ｋｇ以下の外力が作用しても問題ないとされる場合は、このような条件にする。

そして、ロボットＲがワークＷを持上げる場合、−Ｚ方向に４０ｋｇの外力をロボットＲに加えても、Ｚ方向の外力推定値はｆｚ＝（２０−Ａ）−４０＝−２０−Ａ（０≦Ａ＜２０ｋｇ）となる。つまり。外力が加わったと判定されない可能性がある。このように外力判定条件の閾値が十分に大きい場合であっても、外力監視が正しく機能しないことには変わりない。

ただし、ロボットＲに外力が作用していない場合においては、ワークＷを持上げるときの外力推定値はｆｚ＝（２０−Ａ）（０≦Ａ＜２０ｋｇ）である。つまり、実際にロボットＲが外力が作用していないのにも関わらず、外力監視部２６が外力があると誤判定することはない。このように、動作監視無効時外力判定条件の閾値が十分に大きい場合には、動作監視無効時外力判定条件と動作監視有効時外力判定条件とが同じ条件であっても、外力監視の誤判定によってロボットＲを無用に停止させることはない。

このように、本発明において、ロボットＲがワークＷを持上げる動作およびロボットＲがワークＷを載置する動作を行う際には、動作監視有効指令に切替えて、動作監視部２７が有効になるように外力判定条件を変更する。このため、外力監視部２６が、ロボットに外力がかかっていないにもかかわらず外力がかかったと判断してロボットを停止させないようにできる。従って、本発明では、ロボットＲがワークＷを持上げる動作およびロボットＲがワークＷを載置する動作を安全に行うことができる。

ところで、外力判定条件設定部２５は、外力推定値が所定の第一値を超えたとき、所定時間前から現在までの外力推定値の平均値が所定の第二値を超えたとき、所定時間前の外力推定値から現在の外力推定値への変位量が所定の第三値を超えたときの少なくとも一つの条件を含む外力判定条件を有するのが好ましい。つまり、これらのうちの全ての条件を含んでもよいし、いずれか一つのみの条件を含んでいてもよい。従って、外力監視部２６がロボットＲを停止させる条件を適切に設定することができる。

そのような外力判定条件は、例えば「ｆｘ＞３ｋ」、「過去０．１秒のｆｘの平均値が３ｋｇ以上」、「０．１秒前のｆｘと現在のｆｘの差が±３ｋｇ以上」などである。このような条件を用いて、ロボットＲに外力が加わったことを判定する。また、外力推定値が所定の値を超えているかという条件を使用すると、外力の大きさが大きくなったときに、ロボットＲに外力が加わったと判定することができる。

なお、力センサＳの検出値はノイズを含む場合がある。外力推定値に対して公知の手段で移動平均をとることにより、ノイズの影響を除去しつつ、ロボットＲに外力が加わったかどうかを判定することができる。

また、ロボットＲが外部環境に衝突する場合には、ロボットＲがはじめて外部環境に接触してから、安全監視装置がロボットＲに外力が加わったと判断してロボットＲを停止させるまで、外力推定値は変動し続ける。ロボットＲの速度や外部環境の素材等、さまざまな要因によって外力推定値の変動の態様は異なる。このような場合には、できるだけ早いタイミングで安全監視装置がロボットＲに外力が伝わったと判断できるような外力判定条件を設定するのが好ましい。

また、急激に外力推定値が変動するような場合においては、外力推定値の変位量を用いて、ロボットＲに外力が加わったかどうかを判断するのが好ましい。このような場合には、外力推定値の大きさで判定するよりも早いタイミングで、ロボットＲに外力が加わったと判定できる場合がある。なお、前述した条件を適宜組み合わせて外力判定条件としてもよい。

ところで、図５は他の実施形態においてロボットがワークを把持する前の状態を示す図である。図５に示される台Ｔの頂面には、ワークＷに対応した寸法を有する凹部Ｃが形成されている。図５に示されるように、ワークＷは凹部Ｃに部分的に嵌合しており、ワークＷの一部分が台Ｔの頂面から突出している。

このような状況において、ロボットＲがワークＷを持上げるときには、台ＴとワークＷに摩擦力が生じる。そして、摩擦力は力センサＳに作用するので、外力推定値もまた摩擦力に影響を受ける。この摩擦力は、ワークＷの形状の個体差等に依存し、事前に推定することはできない。従って、ワークＷを持上げる場合に、外力が加わったと判定されてロボットＲが停止するのを防ぐような外力判定条件を決めることは難しい。

このような場合には、動作監視状態が有効のときには外力判定を無効にするのが好ましい。言い換えれば、動作監視状態が有効であるときの外力判定条件では、全ての外力推定値に対して、外力でないと判定する条件にする。つまり、動作監視状態が有効のときは、外力監視を無効にする。

これにより、外力が加わっていないのにも関わらず、ロボットＲが停止するのを防ぐことができる。動作監視状態が有効であるときは、動作監視部２７がロボットＲの安全性を監視しているので、外力監視部２６が無効であってもロボットＲの安全性は確保される。

ところで、前述したように動作監視有効時には外力推定値が正しい外力を意味していない可能性がある。このため、外力監視部２６による外力監視が正確に機能しないときには、動作監視状態を有効に切替えるのが好ましい。そして、動作監視有効時外力判定条件を動作監視無効時外力判定条件より緩い条件にしておく。例えば動作監視無効時外力判定条件は外力推定値が第一閾値を超えたときを条件としている場合には、前記動作監視有効時外力判定条件は外力推定値が第一閾値以上の第二閾値を超えたときを条件とする。

これにより、ロボットＲに外力が作用していないにも関わらず、ロボットＲに外力が作用したと外力監視部２６が誤判定するのを避けられる。従って、ロボットＲが誤判定により停止するのを防止できる。

例えば前述したように動作監視無効時外力判定条件を、「｜ｆｚ｜＞３ｋｇ」とする。動作監視有効時には、この条件では、ロボットＲに外力が加わっていないにも関わらず、ロボットＲに外力が加わったと誤判定しうる。このため、動作監視有効時外力判定条件を、「ｆｚ＜−３ｋｇまたはｆｚ＞＋２１ｋｇ」のように、動作監視無効時外力判定条件よりも緩くする。これにより、前述したように誤判定を防ぐことができる。

ところで、動作監視状態切替部２４が動作監視有効指令から動作監視無効指令に切替えるときに、外力推定部２３により推定される外力推定値が所定条件を満たさない場合には、動作監視有効指令から動作監視無効指令への切替えを中止してもよい。

この場合には、動作監視状態を有効から無効に切替えようとした場合に、外力監視が正しく働くかどうかを確認できる。そして、外力監視が正確に機能しない場合には動作監視状態を有効のままにすることができる。つまり、外力監視部２６が正確に機能しない場合には、動作監視部２７を有効にしてロボットＲの安全性を監視し続ける。

前述したように、ロボットＲがワークＷを持上げて、動作監視無効指令が入力された場合に、外力推定値が所定の条件を満たしていると、動作監視状態切替部２４は、動作監視状態を無効に切替える。そして、外力推定値が所定の条件を満たしていない場合には、動作監視状態切替部２４は動作監視状態を有効のままにする。ここで、所定の条件は、例えば「｜ｆｘ｜＜１ｋｇ、｜ｆｙ｜＜１ｋｇ、｜ｆｚ｜＜１ｋｇであること」等である。

例えば動作プログラムの作成の誤り等により、ワークＷが台Ｔから完全に離間する前に動作監視無効指令が入力された場合を考える。この場合には、ｆｚ＝２０−Ａｋｇ（０≦Ａ＜２０ｋｇ）なので、動作監視状態が有効のままで切替わらない。このとき、外力監視が正しく検出しない為、動作監視部２７がロボットＲの安全性を監視し続ける。

ここで、所定の条件として、外力監視部２６が使用する外力判定条件とは別の条件を設定してもよい。上述のように外力監視部２６においては、ロボットＲが動作するときの慣性力を推定する際の誤差も含めて外力判定条件を設定する必要がある。

これに対し、ロボットＲが静止しているときのみ、動作監視状態を有効から無効に切替えることを許すというシステムを構築する場合は、慣性力を推定する際の誤差を含める必要はない。このような場合には、動作監視状態を有効から無効に切替える場合の条件を厳しくしてもよい。また、ロボットＲが動作中なのか停止中なのかに関わらず、ワーク質量の誤差が外力推定値に影響するので、ワーク質量の誤差を考慮して条件を設定してもよい。

ところで、複数種類のワークＷを搬送するときや、台Ｔに凹部Ｃが形成されている場合と凹部Ｃが形成されていない場合とにおいては、それぞれの状況に応じて、ワークＷを持上げる方向や持上げているときに作用する力が異なる。また、ワークＷを持上げるときの動作方向と、ワークＷを載置するときの動作方向も互いに異なる。

このため、動作監視状態有効時外力判定条件および動作監視の評価条件は、それぞれの状況に適した条件であるようにする。そこで、予め複数の動作監視状態有効時外力判定条件および複数の動作監視の評価条件を用意する。そして、それぞれの状況に適した動作監視状態有効時外力判定条件および動作監視の評価条件を使用するのが好ましい。これにより、状況に応じて適した条件を使用することができる。

本発明における動作監視部２７は外力監視が正確に機能しない状態、例えばロボットがワークを持上げる動作およびワークを載置する動作における代替手段としてロボットＲの安全性を監視するものである。そして、外力監視が正確に機能するときには、動作監視状態を無効としている。ここで、一般的に、別の目的などでロボットＲの位置や速度を監視することはよく行われることであり、本発明における動作監視状態が無効の状態においても、ロボットＲの位置や速度を監視してもよい。

２１ 動作プログラム
２２ ワークパラメータ切替部
２３ 外力推定部
２４ 動作監視状態切替部
２５ 外力判定条件設定部
２６ 外力監視部
２７ 動作監視部
Ｃ 凹部
Ｅ エンドエフェクタ
Ｒ ロボット
ＲＣ ロボット制御装置
Ｔ 台
Ｗ ワーク

Claims (5)

1. ロボットに作用する外力を検出する力検出部を備えたロボットを監視する安全監視装置において、
   前記ロボットに把持されるべきワークの質量と質量中心と慣性行列のうち少なくとも一つを含むワークパラメータを切替えるワークパラメータ切替部と、
   該ワークパラメータ切替部により切替えられたワークパラメータを用いて、外部環境から前記ロボットに作用する力またはトルクを外力推定値として推定する外力推定部と、
   前記外力推定値が外力判定条件を満たすときに前記ロボットを停止させる外力監視部と、
   前記ロボットの位置が所定の領域内に含まれているか、前記ロボットの移動方向が所定の方向か、ロボットの動作速度が所定の速度以下であるか、前記ロボットの動作加速度が所定の加速度以下であるかのうちの少なくとも一つの条件が満たされない場合に前記ロボットを停止させる動作監視部と、
   該動作監視部を無効にする動作監視無効指令と前記動作監視部を有効にする動作監視有効指令とを切替える動作監視状態切替部と、
   前記動作監視無効指令に切替えられたときには前記外力判定条件を動作監視無効時外力判定条件に切替えると共に、前記動作監視有効指令に切替えられたときには前記外力判定条件を動作監視有効時外力判定条件に切替える外力判定条件設定部と、を具備する、ロボットの安全監視装置。
2. 前記外力判定条件は、前記外力推定値が所定の第一値を超えたとき、前記外力推定値の移動平均が所定の第二値を超えたとき、所定時間前の前記外力推定値と現在の前記外力推定値との間の変位量が所定の第三値を超えたときの少なくとも一つの条件を含むことを特徴とする請求項１に記載の安全監視装置。
3. 前記動作監視有効指令に切替えられたときには、前記外力監視部を無効にするようにした、請求項１または請求項２に記載の安全監視装置。
4. 前記動作監視無効時外力判定条件は前記外力推定値が第一閾値を超えたときを条件としており、前記動作監視有効時外力判定条件は前記外力推定値が前記第一閾値以上の第二閾値を超えたときを条件としている請求項１または請求項２に記載の安全監視装置。
5. 前記動作監視状態切替部が前記動作監視有効指令から前記動作監視無効指令に切替えるときに、前記外力推定部により推定される前記外力推定値が所定条件を満たさない場合には、前記動作監視有効指令から前記動作監視無効指令への切替えを中止する請求項１から４のいずれか一項に記載の安全監視装置。

Images