(57)【要約】【課題】人と共同して作業するロボットに適した制御装置を提供する。【解決手段】ロボット１の制御装置３は、第１のロボットアーム２及び第２のロボットアーム２を有するロボット１の動作を制御する。制御装置３は、第１のロボットアーム２の先端（エンドエフェクタ１９）と第２のロボットアーム２の先端（エンドエフェクタ１９）との間の距離Ｄを算出する距離算出部４１と、距離算出部４１により算出された距離Ｄが所定値以下であるか否かを監視する距離監視部４２と、を備える。【選択図】図２
2020037144.tif 000002
【特許請求の範囲】
【請求項１】
第１のロボットアーム及び第２のロボットアームを有するロボットの動作を制御する制御装置であって、
前記第１のロボットアームの先端と第２のロボットアームの先端との間の距離を算出する距離算出部と、
前記距離算出部により算出された前記距離が所定値以下であるか否かを監視する距離監視部と、
を備える、ロボットの制御装置。
【請求項２】
前記距離算出部により算出された前記距離が前記所定値以下である場合には、前記第１のロボットアーム及び第２のロボットアームの動作速度が所定速度を超えないように前記ロボットアームの動作を制御する、請求項１に記載のロボットの制御装置。
【請求項３】
前記距離算出部により算出された前記距離が前記所定値以下である場合に、前記第１のロボットアーム及び第２のロボットアームの動作速度が前記所定速度を超えたか否かを監視する速度監視部を更に備え、
前記動作速度が前記所定速度を超えた場合は、前記第１のロボットアーム及び第２のロボットアームの動作を停止させる、請求項２に記載のロボットの制御装置。
【請求項４】
前記第１のロボットアームの先端及び前記第２のロボットアームの先端に作用する外力を検出する外力検出部と、
前記距離算出部により算出された前記距離が所定値以下である場合に、前記外力検出部によって検出された外力に基づいて、所定の監視基準で衝突を検知する力監視部と、
を更に備え、
衝突が検知された場合は、前記第１のロボットアーム及び前記第２のロボットアームの動作を停止させる、請求項１乃至３のいずれか一項に記載のロボットの制御装置。
【請求項５】
前記第１のロボットアームの先端及び前記第２のロボットアームの先端に作用する外力を検出する外力検出部と、
前記距離算出部により算出された前記距離が所定値よりも大きい場合に、前記外力検出部によって検出された外力に基づいて、第１の監視基準で衝突を検知するとともに、前記距離算出部により算出された前記距離が所定値以下である場合に、前記外力検出部によって検出された外力に基づいて、前記第１の監視基準とは異なる監視基準である第２の監視基準で衝突を検知する力監視部と、
を更に備え、
衝突が検知された場合は、前記第１のロボットアーム及び前記第２のロボットアームの動作を停止させる、請求項１乃至４のいずれか一項に記載のロボットの制御装置。
【請求項６】
前記第１ロボットアーム、および、前記第２ロボットアームは、同軸まわりにそれぞれ独立して回転可能に設けられる、請求項１乃至５のいずれか一項に記載のロボットの制御装置。
【請求項７】
前記第１のロボットアームの先端の位置は、前記第１のロボットアームのツールセンターポイントであり、
前記第２のロボットアームの先端の位置は、前記第２のロボットアームのツールセンターポイントであって、
前記距離算出部は、前記第１のロボットアームのツールセンターポイントと前記第２のロボットアームのツールセンターポイントとの間の距離を算出する、請求項１乃至６のいずれか一項に記載のロボットの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、人と共存して作業するロボットの制御装置に関する。
【背景技術】
【０００２】
近年では、生産性向上の観点から、ロボットと作業者が同じ作業空間内で共同して作業を行うことが提案されている。このため、従来から、人と共存して作業するロボットの安全性を監視する技術の開発が進められている。
【０００３】
例えば特許文献１には、外力を監視してロボットの安全性を監視するロボットの安全監視装置が開示されている。制御装置は、ロボットの動作範囲において所定領域を設定し、ロボットの現在位置が所定領域の内外で外力の判定条件を変更し、外力が判定条件を満たす場合はロボットを停止させる。また、制御装置は、ロボットの現在位置が所定領域内に在る場合には、ロボットの移動速度の上限を所定速度に制限する。
【０００４】
特許文献２にはロボットの干渉回避装置が開示されている。ロボットの制御装置は、フランジの先端と人との間の距離を計算するとともに、フランジの先端が人に向かっているかを判断し、人に向かっている場合は、危険対象部（フランジ先端）を離間距離に応じて減速又は緊急停止させる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１７−７７６０８号公報
【特許文献２】特許第５３７０１２７号
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、ロボットと人が共存して作業をする場合、周囲の人間に与える影響を最小限にするためにロボットのアームの位置や姿勢を適切に監視する必要がある。このため、上記従来のロボットの監視装置では、必ずしも人と共同して作業するロボットに最適とは言えないことがあった。
【０００７】
そこで本発明は、人と共同して作業するロボットに適した制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明の一態様に係るロボットの制御装置は、第１のロボットアーム及び第２のロボットアームを有するロボットの動作を制御する制御装置であって、前記第１のロボットアームの先端と第２のロボットアームの先端との間の距離を算出する距離算出部と、前記距離算出部により算出された前記距離が所定値以下であるか否かを監視する距離監視部と、を備えるものである。
【０００９】
例えば双腕ロボットが作業者と同じ作業空間内で共同して作業する場合、２本のロボットアームは独立して動作することや、互いに関連して動作することが可能であるため、作業内容によっては、２本のアームの先端の間隔が狭くなり、周囲の作業者を挟み込んでしまう可能性がある。上記構成によれば、２本のアームの先端により周囲の障害物（例えば作業者）を挟み込む可能性があるか否かを監視することができるので、人と共同して作業するロボットに適した制御装置を提供できる。
【００１０】
上記制御装置が、前記距離算出部により算出された前記距離が前記所定値以下である場合には、前記第１のロボットアーム及び第２のロボットアームの動作速度が所定速度を超えないように前記ロボットアームの動作を制御してもよい。
【００１１】
例えば双腕ロボットが作業者と同じ作業空間内で共同して作業する場合、ロボットアームの動作を速くすると作業性は向上するが、作業者の安全性の確保が問題になる場合がある。一方、ロボットアームの動作を遅くすると作業者の安全性は確保できるが、作業性は低下する。このようにロボットの作業性と作業者の安全性は、トレードオフの関係にある。上記構成によれば、２本のアームの先端により周囲の障害物を挟み込む可能性が場合には、動作速度が所定速度を超えないようにアームの動作を制御する。一方で、２本のアームの先端により周囲の物を挟み込む可能性がある場合にはこれらのアームを可能な限り高速度で動作させることができる。これにより、例えばロボットの近くの作業者が２本のアームの先端に挟まれた場合でも、リンクが高速で衝突することはない。従って、ロボットの作業性と作業者の安全性の両立を図ることができる。
【００１２】
上記制御装置が、前記距離算出部により算出された前記距離が前記所定値以下である場合に、前記第１のロボットアーム及び第２のロボットアームの動作速度が前記所定速度を超えたか否かを監視する速度監視部を更に備え、前記動作速度が前記所定速度を超えた場合は、前記第１のロボットアーム及び第２のロボットアームの動作を停止させてもよい。
【００１３】
上記構成によれば、２本のアームの先端により周囲の障害物を挟み込む可能性がある場合には、動作速度が所定速度を超えた場合は、アームの動作を停止させることができるので、作業者の安全性が更に向上する。
【００１４】
上記制御装置が、前記第１のロボットアームの先端及び前記第２のロボットアームの先端に作用する外力を検出する外力検出部と、前記距離算出部により算出された前記距離が所定値以下である場合に、前記外力検出部によって検出された外力に基づいて、所定の監視基準で衝突を検知する力監視部と、を更に備え、衝突が検知された場合は、前記第１のロボットアーム及び前記第２のロボットアームの動作を停止させてもよい。
【００１５】
上記構成によれば、２本のアームの先端により周囲の物を挟み込む可能性がある場合には２本のアームの先端に作用する外力に基づいて衝突を検知し、衝突を検知した後はロボットの動作を停止させることができる。作業者の安全性を向上させることができる。
【００１６】
上記制御装置が、前記第１のロボットアームの先端及び前記第２のロボットアームの先端に作用する外力を検出する外力検出部と、前記距離算出部により算出された前記距離が所定値よりも大きい場合に、前記外力検出部によって検出された外力に基づいて、第１の監視基準で衝突を検知するとともに、前記距離算出部により算出された前記距離が所定値以下である場合に、前記外力検出部によって検出された外力に基づいて、前記第１の監視基準とは異なる監視基準である第２の監視基準で衝突を検知する力監視部と、を更に備え、衝突が検知された場合は、前記第１のロボットアーム及び前記第２のロボットアームの動作を停止させてもよい。
【００１７】
上記構成によれば、２本のアームの先端により周囲の物を挟み込む可能性がない場合には第１の監視基準（例えば衝突感度を低く設定）で衝突を検知する。一方、２本のアームの先端により周囲の物を挟み込む可能性がある場合には第２の監視基準（例えば衝突感度を高く設定）で衝突を検知する。つまり、２本のアーム先端の間隔に応じて衝突感度を変更することができるので、ロボットの作業性と作業者の安全性の両立を図ることができる。
【００１８】
尚、前記第１ロボットアーム、および、前記第２ロボットアームが、同軸まわりにそれぞれ独立して回転可能に設けられてもよい。
【００１９】
尚、前記第１のロボットアームの先端は、前記第１のロボットアームのツールセンターポイントであり、前記第２のロボットアームの先端は、前記第２のロボットアームのツールセンターポイントであって、前記距離算出部が、前記第１のロボットアームのツールセンターポイントと前記第２のロボットアームのツールセンターポイントとの間の距離を算出してもよい。
【発明の効果】
【００２０】
本発明によれば、人と共同して作業するロボットに適した制御装置を提供することができる。
【図面の簡単な説明】
【００２１】
【図１】図１は、第１実施形態に係るロボットの構成を概略的に示す正面図である。
【図２】図２は、図１のロボットの作業の一例を示す平面図である。
【図３】図３は、図１のロボットの全体構成を示すブロック図である。
【図４】図４は、図３の監視装置の構成を示すブロック図である。
【図５】図５は、ロボットの監視動作の一例を示すフローチャートである。
【図６】図６は、第２実施形態に係る監視装置の構成を示すブロック図である。
【図７】図７は、ロボットの監視の動作の一例を示すフローチャートである。
【図８】図８は、図７の監視動作の変形例を示すフローチャートである。
【発明を実施するための形態】
【００２２】
本発明の実施の形態について、図面を参照しつつ説明する。以下では全ての図を通じて同一又は相当する要素には同一の参照符号を付して、その重複する説明を省略する。
【００２３】
（第１実施形態）
図１は、第１実施形態に係るロボット１の構成を概略的に示す正面図である。図１に示すように、ロボット１は、台車８に固定されたベース９と、ベース９に支持された一対のロボットアーム（以下、単に「アーム」と記載する場合がある）２、２と、ベース９内に収納された、制御装置３と、を備えている。本実施形態のロボット１は水平多関節型のアーム２，２を備えた双腕ロボットである。以下では、一対のアーム２，２を広げた方向を左右方向と称し、基軸１６の軸心に平行な方向を上下方向と称し、左右方向および上下方向に直交する方向を前後方向と称する。各アーム２は、アーム部２０と、リスト部１７と、エンドエフェクタ（図示せず）と、を備えている。なお、２本のアーム２は、実質的に同じ構造であってもよい。また、２本のアーム２は、独立して動作したり、互いに関連して動作したりすることができる。本実施形態のロボット１は、例えば、生産ラインに導入され、作業者と同じ作業空間で共同して作業を行う。
【００２４】
本実施形態のロボット１は同軸双腕型のロボットである。２本のアーム２，２のそれぞれは、ベース９に垂直な基軸１６に同軸に配置されかつベース９に対して独立して回転軸線Ａ１回りに回動可能に構成される。アーム部２０は、本例では、第１リンク２０ａおよび第２リンク２０ｂとで構成されている。第１リンク２０ａは、ベース９の上面に固定された基軸１６と回転関節Ｊ１により連結され、基軸１６の軸心を通る回転軸線Ａ１まわりに回動可能である。つまり、２本のアーム２、２の第１リンク２０ａ、２０ａの回転軸線Ａ１は同一直線上にあり、一方のアーム２の第１リンク２０ａと他方のアーム２の第１リンク２０ａとは上下に高低差を設けて配置されている。第２リンク２０ｂは、第１リンク２０ａの先端と回転関節Ｊ２により連結され、第１リンク２０ａの先端に規定された回転軸線Ａ２まわりに回動可能である。
【００２５】
リスト部１７は、直動関節Ｊ３及び回転関節Ｊ４を有する。リスト部１７は、直動関節Ｊ３によって、第２リンク２０ｂに対し昇降移動である。リスト部１７は、回転関節Ｊ４によって、第２リンク２０ｂに対し垂直な回転軸線Ａ３まわりに回動可能である。リスト部１７の先端には、メカニカルインターフェース１８が取り付けられる。メカニカルインターフェース１８は、直動関節Ｊ３及び回転関節Ｊ４を介して、第２リンク２０ｂの先端と連結されている。メカニカルインターフェース１８には作業用のエンドエフェクタ（図示せず）が取り付けられる。
【００２６】
上記構成の各アーム２は、各関節Ｊ１〜Ｊ４を有する。各関節Ｊ１〜Ｊ４は、例えばサーボ機構（図示しない）により駆動される。サーボ機構は、アーム２を変位駆動するための駆動部と、駆動部の動力をアーム２に伝達するための伝達機構とを含む。本実施の形態では、駆動部は、例えばサーボモータによって実現され、各サーボモータにはモータの回転角度位置を検出するエンコーダ等の位置センサがそれぞれ設けられる（図示しない）。ここで回転角度位置とは、各サーボモータの関節座標系における各関節の角度の位置である。制御装置３は、サーボモータを位置制御することにより、左右のアーム２の動作を任意の速度で制御するように構成される。
【００２７】
図２は、図１のロボット１の作業の一例を示す平面図である。このロボット１は、人一人分に相当する限られたスペース（例えば６１０ｍｍ×６２０ｍｍ）に設置することができる。図２に示すように、ロボット１は、生産ラインに導入され、作業者と同じラインで共同して作業をする。ロボット１は、作業台１００の上で作業者と同様な作業を行う。ロボット１の左右のアーム２，２の各々の先端（メカニカルインターフェース１８）には作業用のエンドエフェクタ１９がそれぞれ連結されている。ＴＰは各エンドエフェクタ１９のツールセンターポイントである。本実施形態では、左右のエンドエフェクタ１９は、同じ構造である。作業台１００上のロボットの作業スペースの左右両側の領域は、それぞれ作業者が各自の作業を行う作業スペースである。本実施形態では、作業台１００の上には、４種類のワークＷ１、Ｗ２，Ｗ３，Ｗ４が配置されている。左側に位置する作業者が、ロボット１に材料部材であるワークＷ１を供給する。ロボット１は供給されたワークＷ１に対して、第１の部品であるワークＷ２及び第２の部品であるワークＷ３を取り付けて、ワークＷ４を完成させる。右側に位置する作業者は、完成されたワークＷ４に対して次の作業行程を行う。
【００２８】
ロボット１は基準座標系（以下、ベース座標系という）を持っている。この座標系は、例えば、ベース（基台）９の設置面と第１関節Ｊ１の回転軸線Ａ１との交点が原点であり、第１関節Ｊ１の回転軸線がＺ軸であり、Ｚ軸に直交する任意の軸がＸ軸であり、Ｚ軸及びＸ軸に直交する軸がＹ軸である。ロボット１のアーム２の可動範囲は、第１関節Ｊ１を中心とした円形領域である（図示せず）。各アーム２の動作範囲は、このベース座標系を基準として設定される。本実施形態では、動作領域は、少なくとも、ロボット１の正面に配置された作業台１００を覆うように設定される。
【００２９】
このように、双腕型のロボット１が作業者と同じ作業空間内で共同して作業する場合、２本のアーム２，２は独立して動作することや、互いに関連して動作することが可能である。このため、アーム２，２の位置や姿勢によっては、アーム２，２の先端（エンドエフェクタ１９，１９）の間隔が狭くなり、ロボット１の近くに位置する作業者が、アーム２，２の先端の間に挟まれる可能性がある。
【００３０】
そこで、本実施形態の制御装置３は、周辺の作業者の安全を確保するために、ロボット１の動作を監視する監視機能を備えている。図３は、ロボット１の全体構成を示すブロック図である。図３に示すように、ロボット１は、ロボットアーム２と、制御装置３と、監視装置４を備える。なお、ロボットアーム２は２本であるが、ここでは、説明の簡単化のために、１本のみ示している。ロボットアーム２は、１以上の関節Ｊと、関節を介して連結された複数のリンクと、各関節Ｊに設けられた駆動用のサーボモータＭを備える。各サーボモータＭには、モータの位置（回転子の基準回転角度位置に対する回転角度位置）を検出するエンコーダ等の位置センサＥと、モータを駆動する電流を検出する電流センサ５が取り付けられる。
【００３１】
制御装置３は、ロボットアーム２とケーブルＣ（太字で図示）を介して接続される。ここでケーブルＣは関節ＪのサーボモータＭやブレーキ（図示せず）等に電源を供給するための電源ライン、サーボモータＭに取り付けられた位置センサＥからのセンサ信号を受信するための信号ライン等が含まれる。また、制御装置３は監視装置４と通信ケーブル（図示しない）を介して接続される。ここで通信ケーブルは、例えばＲＳ４２２等のシリアル通信用のケーブルである。本実施形態では、制御装置３は、通信ケーブルを介して、監視装置４に監視信号（位置センサの検出信号）を供給するとともに、監視装置４から速度制限指令を受信し、これに従ってロボットアーム２の動作速度を変更するように構成される。ここでロボットアーム２の動作速度とは、ロボットアーム２を構成するサーボモータＭの回転速度（以下、「モータ速度」ともいう）を意味する。
【００３２】
制御装置３は、演算処理器６、サーボアンプ７、メモリ、入出力インタフェース、通信インタフェース等を備えたロボットコントローラである。演算処理器６は、電流指令値生成部６１と、速度制限値設定部６２とを備える。ここで電流指令値生成部６１及び速度制限値設定部６２は、演算処理器６において、所定のプログラムが実行されることによって、実現される機能ブロックである。電流指令値生成部６１は、ロボットの動作プログラムに基づいて、関節Ｊを駆動するサーボモータＭの位置指令値を生成し、生成した位置指令値と位置センサからの検出値（実際値）の偏差に基づいて速度指令値を生成する。そして、生成した速度指令値と速度現在値の偏差に基づいてトルク指令値（電流指令値）を生成し、サーボアンプ７に出力する。サーボアンプ７は、サーボモータＭに対応して設けられ、与えられる電流指令値に基づいて電流を発生し、ケーブルＣを介して発生した電流をサーボモータＭに供給する。つまり、各サーボアンプ７は電流指令値に応じてサーボモータＭの駆動電流を発生する増幅器である。制御装置３は、位置指令値に基づいて各関節Ｊに設けられたサーボモータＭを位置制御することにより、各ロボットアーム２の動作を制御するように構成される。
【００３３】
速度制限値設定部６２は、監視装置４から受信した速度制限指令に基づいて、各アーム２，２のモータ速度が所定の速度を超えないように速度制限値を設定する。本実施形態では、速度制限値設定部６２は、初期値として、速度制限値を第１速度（例えば８００ｍｍ／ｓ）に設定している。電流指令値生成部６１は、速度制限値設定部６２により設定された速度制限値を超えないように、生成した速度指令値に制約を与える。
【００３４】
図４は、図３の監視装置４の構成を示すブロック図である。図４に示すように、監視装置４は、距離算出部４１と、距離監視部４２と、速度制限値生成部４３と、速度算出部４４と、速度監視部４５と、停止信号生成部４６と、を備える。ここで監視装置４は、１以上のプロセッサ、メモリ、入出力インタフェース、通信インタフェース等を備えたコンピュータである。各部（４１〜４６）は、プロセッサにおいて、所定のプログラムが実行されることによって、実現される機能ブロックである。
【００３５】
距離算出部４１は、一方のアーム２の先端と他方のアーム２の先端との間の距離Ｄを算出するように構成される。本実施形態では、距離算出部４１は、各サーボモータＭの回転角度位置（位置センサＥの検出信号）及び予め設定された各リンクの長さや形状等の情報に基づいて、各アーム２の先端の位置を算出し、両アーム２先端の間の距離Ｄを算出する。本実施形態では、アーム２，２の先端の位置は、エンドエフェクタ１９，１９のツールセンターポイントＴＰ，ＴＰである（図２参照）。距離算出部４１は、ロボット１のベース座標系におけるツールセンターポイントＴＰ，ＴＰの各位置座標を算出し、エンドエフェクタ１９，１９のツールセンターポイントＴＰ，ＴＰ間の距離Ｄを算出する。
【００３６】
距離監視部４２は、距離算出部４１により算出された距離Ｄが所定値以下であるか否かを監視するように構成される。ここで所定値は、アーム２，２の先端によって周囲の障害物（例えば作業者）を挟み込む可能性のある値に設定される。本実施形態では所定値は１０ｃｍに設定される。尚、監視装置４は、監視対象である距離Ｄの設定値を、例えば管理者により任意の値に調整可能な入力手段（図示せず）を備える。本実施形態では所定値は１０ｃｍに設定されるが、ロボット１の周囲の状況や想定される障害物の大きさによって適宜設定されてもよい。距離監視部４２は、各サーボモータの回転角度位置及び予め設定された各リンクの長さや形状等の情報に基づいて、ロボット１の３次元モデルを生成するように構成されていてもよい。
【００３７】
速度制限値生成部４３は、距離Ｄが所定値以下である場合には、各アーム２，２のモータ速度が所定速度を超えないように速度制限指令を生成し、制御装置３に送信するように構成される。本実施形態では、速度制限値生成部４３は、距離Ｄが所定値以下である場合には、各アーム２，２のモータ速度が第１速度（初期値）よりも低い第２速度を超えないような速度制限指令を生成し、制御装置３に送信するように構成される。
【００３８】
速度算出部４４は、各サーボモータＭの回転角度位置（位置センサＥの検出信号）に基づいて、各サーボモータＭの速度（モータ速度）を演算する。ここでは監視信号に含まれる各サーボモータＭの回転角度位置が、通信ケーブルを介して制御装置３から監視装置４に送信され、速度算出部４４に入力される。速度算出部４４は、算出結果を速度監視部４５に出力するように構成されている。
【００３９】
速度監視部４５は、各アーム２，２のモータ速度が所定の速度を超えているか否かを監視する。本実施形態では、速度監視部４５は、距離算出部４１により算出された距離Ｄが所定値以下である場合に、各ロボットアーム２の関節Ｊ１〜Ｊ４に設けられたサーボモータＭのモータ速度のうち、いずれかのモータ速度が第２速度を超えているか否かを監視する。
【００４０】
停止信号生成部４６は、距離Ｄが所定値以下である場合に、速度監視部４５によりモータ速度が第２速度を超えていると判定された場合にはロボット１の停止信号を生成し、これを制御装置３に供給する。
【００４１】
次に、監視装置４によるロボット１の監視動作について図５のフローチャートを参照しつつ説明する。図５に示すように、まず、監視装置４は、制御装置３から所定期間ごとに送信される監視信号の受信を待機する（図５のステップＳ１１）。本実施形態では、監視信号は、ロボットアーム２の位置制御において使用される各関節Ｊに設けられた位置センサＥの検出信号を含む。
【００４２】
次に、監視装置４は、監視信号を受信したときは、距離算出部４１により２本のアーム２，２先端（ＴＰ，ＴＰ）の間の距離Ｄ（図２参照）を算出する（図５のステップＳ１２）。距離算出部４１は、各サーボモータＭの回転角度位置（位置センサＥの検出信号）及び予め設定された各リンクの長さや形状等の情報に基づいて、各アーム２の先端（ＴＰ，ＴＰ）の位置を算出し、両アーム２先端（ＴＰ，ＴＰ）の間の距離Ｄを算出する。
【００４３】
次に、距離監視部４２は、距離算出部４１により算出された距離Ｄが所定値以下であるか否かを監視する（図５のステップＳ１３）。本実施形態では、距離監視部４２は、距離監視部４２は、距離Ｄが１０ｃｍ以下である場合にはアーム２，２の先端によって周囲の阿形者を挟み込む可能性があると判定する。一方、距離Ｄが１０ｃｍよりも長い場合にはアーム２，２の先端によって周知の作業者を挟み込む可能性はないと判定する。
【００４４】
次に、速度制限値生成部４３は、距離監視部４２によりアーム２，２の先端によって周囲の作業者を挟み込む可能性があると判定された場合（図５のステップＳ１３でＹＥＳ）は、モータ速度が第１速度よりも低い第２速度を超えないような速度制限指令を生成し、これを制御装置３に供給する（図５のステップＳ１４）。ここで第２速度は、ＩＳＯ１０２１８−１に低速制御として規定されている２５０ｍｍ／ｓである。制御装置３は、モータ速度が第２速度を超えないようにアーム２，２の動作を制御する。これにより、例えばロボットの近くの作業者の一部（例えば手首）がアーム２，２の先端の間に挟まれた場合でも、エンドエフェクタ１９，１９が高速で衝突することはない。
【００４５】
更に、本実施形態では、速度監視部４５は、アーム２，２の動作速度がステップＳ１４で制限された第２速度を超えているか否かを監視する（図５のステップＳ１５）。本実施形態では、速度監視部４５は、距離Ｄが所定値以下である場合に、アーム２，２の関節Ｊ１〜Ｊ４に設けられたサーボモータＭのうち、いずれかのモータ速度が第２速度を超えているか否かを監視する。
【００４６】
次に、停止信号生成部４６は、速度監視部４５によりモータ速度が第２速度を超えていると判定された場合（図５のステップＳ１５でＮＯ）にはロボット１の停止信号を生成し、これを制御装置３に供給する（図５のステップＳ１６）。制御装置３はロボット１の動作を停止させる。
【００４７】
一方、速度制限値生成部４３は、距離監視部４２により、アーム２，２の先端によって周囲の作業者を挟み込む可能性はないと判定された場合（図５のステップＳ１３でＮＯ）には、速度制限指令を生成しない。ここで速度制限値の初期値は、第１速度（例えば８００ｍｍ／ｓ）に設定されているので、制御装置３は、モータ速度が第１速度を超えないようにアーム２，２の動作を高速で制御する。これにより、ロボット１の能力を最大限発揮させることができる。制御装置３は、以上のような動作をロボット１の作業が終了するまで繰り返し行う。
【００４８】
一般に、図２に示すように、ロボット１と作業者が同じ作業空間内で共同して作業する場合、アーム２，２の動作を速くすると作業性は向上するが、作業者の安全性の確保が問題になる場合がある。一方、アーム２，２の動作を遅くすると作業者の安全性は確保できるが、作業性は低下する。このようにロボット１の作業性と作業者の安全性は、トレードオフの関係にある。
【００４９】
そこで、本実施形態によれば、アーム２，２の先端の間の距離Ｄが所定値よりも大きい場合（図５のステップＳ１３でＮＯ）には、アーム２，２の先端により周囲の作業者を挟み込む可能性が無いと判断しアーム２，２を可能な限り高速度（第１速度）で動作させる。一方で、距離Ｄが所定値以下では、アーム２，２の先端により周囲の作業者を挟み込む可能性が有ると判断しアーム２，２を低速度（第２速度）で動作させる。これにより、ロボット１の周囲の作業者の一部（例えば手首）がロボットアーム２，２の先端に挟まれた場合でも、エンドエフェクタ１９，１９が高速で衝突することはない。従って、ロボットの作業性と作業者の安全性の両立を図ることができる。
【００５０】
さらに、本実施形態では、距離Ｄが所定値以下である場合に、ロボットアーム２の動作速度が第２速度を超えた場合は、ロボットアーム２の動作を停止させるので、作業者の安全性が更に向上する。人と共同して作業するロボットに適した制御装置３を提供することができる。
【００５１】
（第２実施形態）
次に、第２実施形態について説明する。以下では、第１実施形態と共通する構成の説明は省略し、相違する構成についてのみ説明する。
【００５２】
図６は、第２実施形態に係る監視装置の構成を示すブロック図である。図６に示すように、本実施形態では、第１実施形態（図４）と比較すると、監視装置４Ａが、速度制限値生成部４３、速度算出部４４及び速度監視部４５の代わりに、駆動トルク推定部４７、外力検出部４８及び力監視部４９を備える点が異なる。本実施形態の監視装置４Ａは、速度監視機能に代えて、力監視機能を備えている。このため、本実施形態では、制御装置３から監視装置４に送信される監視信号は、位置センサＥの検出信号、及び、電流センサ５で検出されたセンサ電流値を含む。
【００５３】
駆動トルク推定部４７は、位置センサＥにより算出された回転角度位置から、ロボット１の関節のサーボモータを駆動するのに必要な駆動トルクを推定する。駆動トルク推定部４７は、本実施形態では、重力トルク、慣性力トルク、及び摩擦力トルクをそれぞれ算出し、これらを加算することにより、駆動トルクの推定値を算出する。ここで重力トルクは各リンクの重量に打ち勝って姿勢を維持するためのトルクである。慣性力トルクはリンクの慣性に打ち勝つために必要なトルクである。摩擦力トルクは減速機の摩擦に打ち勝つために必要なトルクである。尚、本実施形態では、駆動トルク推定部４７は監視装置４に実装されるような構成であるが、制御装置３に実装されてもよい。駆動トルク推定値は、電流センサ５で検出されたセンサ電流値及び位置センサＥで検出された回転角度位置を含むセンサ信号とともに、監視信号として制御装置３から監視装置４に送信されてもよい。
【００５４】
外力検出部４８は、アーム２，２の先端に作用する外力を検出する。本実施形態では外力検出部４８は、電流センサ５で検出された各サーボモータＭを流れるセンサ電流値をトルク値に変換する。そして、センサ電流値から変換されたトルク値から、駆動トルク推定部４７から入力された駆動トルクの推定値を減算し、これを外乱トルクとして算出する。そして、この外乱トルク値を用いてアーム２の先端に働く外力を算出し、これを力監視部４９に出力する。具体的には、外力検出部４８は、外乱トルクτｄから各アーム２の先端（図２のＴＰ）に働く外力ｆｄを、仮想仕事の原理によって次式（３）のように求める。
【００５５】
ｆｄ＝（ＫＴ）−１τｄ・・・（３）
ここでＫはヤコビ行列であり、ロボット１のベース座標系と関節座標系との間の微小変位関係を表現した行列である。ヤコビ行列Ｋについて、誤差Δｘと関節角差分Δθには式（４）の関係が成立している。
【００５６】
Δｘ＝ＫΔθ・・・・・・（４）
このように外力検出部４８は、式（３）のように外乱トルクτｄにヤコビ行列Ｋの転置行列ＪＴの逆行列を乗じることによりアーム２，２の先端に作用する外力ｆｄを算出し、これを力監視部４９に出力する。尚、式（３）の外力ｆｄはロボットアーム２の先端で作用していると想定したときの外力である。外力ｆｄがアーム２の先端以外を作用点としている場合は、外力ｆｄを実際の作用点での外力に座標変換してもよい。
【００５７】
力監視部４９は、距離算出部４１により算出された距離Ｄ（図２参照）が所定値よりも大きい場合、外力検出部４８によって検出された外力に基づいて、第１の監視基準で衝突を検知するように構成される。具体的には、力監視部４９は、外力検出部４８から入力された外力の値ｆｄの微分値に比例した値ｆ’ｄをロボットアーム２の先端に働く衝撃力として算出し、ロボットアーム２の先端に働く衝撃力の値｜ｆ’ｄ｜が予め設定された第１閾値ｆｔｈ１を超えたか否かを判定し、第１閾値ｆｔｈ１を超えたときにロボットアーム２の先端に障害物が衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力するように構成されている。第１閾値ｆｔｈ１との比較対象である衝撃力の値｜ｆ’ｄ｜は外力ｆ’ｄのスカラ値である。尚、力監視部４９は、外力検出部４８から入力された外力の値｜ｆｄ｜が予め設定された閾値を超えたか否かを判定して衝突を検知してもよい。尚、監視装置４は、例えば管理者により、衝突検知の際の閾値を任意の値に調整可能な入力手段（図示せず）を備える。
【００５８】
停止信号生成部４６は、力監視部４９から衝突検知信号が入力された場合にはロボット１の停止信号を生成し、これを制御装置３に出力する。
【００５９】
次に、監視装置４Ａによるロボット１の監視動作について図７のフローチャートを参照しつつ説明する。図７のステップＳ２１からステップＳ２３までの距離算出部４１及び距離監視部４２の動作は、図５のステップＳ１１からステップＳ１３までの動作と同じであるので、説明を省略する。
【００６０】
図７に示すように、力監視部４９は、距離Ｄが所定値以下である場合（図７のステップＳ２３でＹＥＳ）、外力検出部４８によって検出された外力に基づいて、第１の監視基準で衝突を検知する（図７のステップＳ２４）。具体的には、力監視部４９は、外力検出部４８から入力された外力の値｜ｆｄ｜が予め設定された第１閾値ｆｔｈ１を超えたか否かを判定し、第１閾値ｆｔｈ１を超えたときにロボットアーム２の先端に障害物が衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力する。このように、力監視部４９は、距離監視部４２によりアーム２，２の先端によってロボット１の周囲に存在する障害物（例えば作業者）を挟み込む可能性があると判断された場合、アーム先端に作用する外力に基づいて衝突を検知する。
【００６１】
そして、停止信号生成部４６は、力監視部４９から衝突検知信号が入力された場合（図７のステップＳ２４でＹＥＳ）にはロボット１の停止信号を生成し（図７のステップＳ２５）、これを制御装置３に出力する。これにより、制御装置３は、ロボット１の動作を停止させることができる。
【００６２】
本実施形態によれば、アーム２，２先端により周囲の物を挟み込む可能性のある場合にはアーム２，２先端に作用する外力に基づいて衝突を検知し、衝突を検知した後はロボット１の動作を停止させることができるので、作業者の安全性を向上させることができる。
【００６３】
＜変形例＞
次に、本実施形態の力監視機能の変形例について説明する。本変形例では、距離Ｄに応じて２つの異なる監視基準で衝突を検知する。具体的には、図６の力監視部４９が、距離算出部４１により算出された距離Ｄが所定値よりも大きい場合、外力検出部４８によって検出された外力に基づいて、第１の監視基準で衝突を検知するように構成される。具体的には、力監視部４９は、外力検出部４８から入力された外力の値｜ｆｄ｜が予め設定された第１閾値ｆｔｈ１を超えたか否かを判定し、第１閾値ｆｔｈ１を超えたときにロボットアーム２の先端が障害物に衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力するように構成されている。本実施形態では第１閾値ｆｔｈ１は１００Ｎに設定される。第１閾値ｆｔｈ１との比較対象である外力の値｜ｆｄ｜は外力ｆｄのスカラ値である。
【００６４】
一方、力監視部４９は、力監視部４９は距離算出部４１により算出された距離Ｄが所定値以下である場合、外力検出部４８によって検出された外力に基づいて、第１の監視基準とは異なる監視基準である第２の監視基準で衝突を検知するように構成される。具体的には、力監視部４９は、外力検出部４８から入力された外力の値ｆｄの微分値に比例した値ｆ’ｄをロボットアーム２の先端に働く衝撃力として算出し、ロボットアーム２の先端に働く衝撃力の値｜ｆ’ｄ｜が予め設定された第２閾値ｆｔｈ２超えたか否かを判定し、第２閾値ｆｔｈ２を超えたときにロボットアーム２の先端が障害物に衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力するように構成されている。第２閾値ｆｔｈ２との比較対象である衝撃力の値｜ｆ’ｄ｜は外力ｆ’ｄのスカラ値である。
【００６５】
図８は、本変形例の監視動作を示すフローチャートである。図８に示すように、力監視部４９は、距離Ｄが所定値を超えた場合には（図８のステップＳ２３でＮＯ）、外力検出部４８によって検出された外力に基づいて、第１の監視基準で衝突を検知する（図８のステップＳ２４−１）。具体的には、力監視部４９は、外力検出部４８から入力された外力の値｜ｆｄ｜が予め設定された第１閾値ｆｔｈ１を超えたか否かを判定し、第１閾値ｆｔｈ１を超えたときにロボット１が衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力する。このように、力監視部４９は、距離監視部４２によりアーム２，２の先端によってロボット１の周囲に存在する障害物（例えば作業者）を挟み込む可能性はないと判断された場合は、衝突感度を低く設定し衝突を検知する。
【００６６】
一方、力監視部４９は、距離Ｄが所定値以下である場合（図８のステップＳ２３でＹＥＳ）、外力検出部４８によって検出された外力に基づいて、第１の監視基準とは異なる監視基準である第２の監視基準で衝突を検知する（図８のステップＳ２４−２）。具体的には、力監視部４９は、外力検出部４８から入力された外力の値ｆｄの微分値に比例した値ｆ’ｄをロボットアーム２の先端に働く衝撃力として算出し、ロボットアーム２の先端に働く衝撃力の値｜ｆ’ｄ｜が予め設定された第２閾値ｆｔｈ２を超えたか否かを判定し、第２閾値ｆｔｈ２を超えたときにロボットアーム２の先端が障害物に衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力する。このように、力監視部４９は、距離監視部４２によりアーム２，２の先端によってロボット１の周囲に存在する障害物（例えば作業者）を挟み込む可能性があると判断された場合は、衝突感度を高く設定し衝突を検知する。
【００６７】
そして、停止信号生成部４６は力監視部４９から衝突検知信号が入力された場合には、ロボット１の停止信号を生成し（図８のステップＳ２５）、これを制御装置３に出力する。これにより、制御装置３は、ロボット１の動作を停止させることができる。
【００６８】
本変形例によれば、２本のアーム２，２の先端により周囲の物を挟み込む可能性がない場合には第１の監視基準（衝突感度を低く設定）で衝突を検知する。一方、２本のアーム２，２の先端により周囲の物を挟み込む可能性がある場合には第２の監視基準（衝突感度を高く設定）で衝突を検知する。つまり、２本のアーム２，２の先端の間隔に応じて衝突感度を変更することができるので、ロボットの作業性と作業者の安全性の両立を図ることができる。
【００６９】
尚、本実施形態では、衝突を検知した後はロボットの動作を停止させるようにしたが、周囲の作業者又は管理者に報知するようにしてもよい。
【００７０】
また、本実施形態の監視装置４Ａは、距離Ｄに応じてロボットアーム２，２に作用する外力に基づいて衝突検知を行う力監視機能を備えたが、第１実施形態の速度監視機能を組み合わせてもよい。
【００７１】
尚、本実施形態では、力センサを用いることなく、サーボモータの電流値に基づいてロボットアーム２の先端に働く外力を計算するので、安価且つ高精度に衝突を検知することができる。これにより、ロボットとその周辺で作業する作業者の共同作業における利便性が更に向上する。
【００７２】
（その他の実施形態）
尚、上記各実施形態では、本実施形態では、距離算出部４１は、各サーボモータＭの回転角度位置（位置センサＥの検出信号）及び予め設定された各リンクの長さや形状等の情報に基づいて、各アーム２の先端の位置を算出し、両アーム２先端の間の距離Ｄを算出するように構成されたが、これに限られない。例えば両アーム２先端に距離センサを取り付けて、距離センサの検出値に基づいて両アーム２先端の間の距離Ｄを算出するようにしてもよい。
【００７３】
尚、上記各実施形態では、アーム２，２の先端の位置は、エンドエフェクタ１９，１９のツールセンターポイントＴＰ，ＴＰであり、距離Ｄは、エンドエフェクタ１９，１９のツールセンターポイントＴＰ，ＴＰ間の距離であったが（図２参照）、これに限られない。アーム２，２の先端の位置は、ツールセンターポイント以外の任意の点でもよい。また、アーム先端に設けられたフランジ上に定義されたフランジ座標系の任意の点（例えば原点）でもよい。
【００７４】
尚、上記各実施形態のロボット１は、水平多関節型のアームを備えた双腕ロボットであったが、垂直多関節型のアームを備えた双腕ロボットであってもよい。
【００７５】
尚、上記各実施形態のロボット１は、２本のアーム２，２を備えた１台の双腕ロボットであったが、アーム先端の間の距離を定義することができれば、これに限られない。例えば２台の単腕ロボットのアーム先端の間の距離を監視してもよいし、３本以上のロボットアームを備えたロボットシステムにおいて少なくとも２本のアーム先端の間の距離を監視してもよい。
【００７６】
尚、上記各実施形態の監視装置４，４Ａは制御装置３と別々に設けたが、制御装置３に含まれていてもよい。例えば制御装置３の演算処理器６において、監視装置４の各部の機能ブロックが実行されるように構成されていてもよい。
【００７７】
上記説明から、当業者にとっては、本発明の多くの改良や他の実施形態が明らかである。従って、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び機能の双方又は一方の詳細を実質的に変更できる。
【産業上の利用可能性】
【００７８】
本発明は、人と共存して作業するロボットに有用である。
【符号の説明】
【００７９】
１  ロボット
２  ロボットアーム
３  制御装置
４，４Ａ  監視装置
５  電流センサ
６  演算処理器
７  サーボアンプ
８  台車
９  ベース
１７  リスト部
１８  メカニカルインターフェース
１９  エンドエフェクタ
２０  アーム部
２０ａ  第１リンク
２０ｂ  第２リンク
４１  距離検出部
４２  距離監視部
４３  速度制限値生成部
４４  速度算出部
４５  速度監視部
４６  停止信号生成部
４７  駆動トルク推定部
４８  外力検出部
４９  力監視部
６１  電流指令値生成部
６２  速度制限値設定部
１００  作業台
Ｊ  関節
Ｍ  サーボモータ
Ｅ  エンコーダ（位置センサ）
Ｃ  ケーブル
ＴＰ  ツールセンターポイント
Ｄ  アーム先端の間の距離
【図１】
2020037144.tif 000003
【図２】
2020037144.tif 000004
【図３】
2020037144.tif 000005
【図４】
2020037144.tif 000006
【図５】
2020037144.tif 000007
【図６】
2020037144.tif 000008
【図７】
2020037144.tif 000009
【図８】
2020037144.tif 000010