JP2019181611A - ロボットの制御装置 - Google Patents

Abstract

【課題】人と共同して作業するロボットに適した制御装置を提供する。【解決手段】ロボット１の制御装置３は、一の回転軸線を介して連結された複数のリンク、および、前記回転軸線に設けられた駆動用のモータＭを有するロボットアーム２の動作を制御する。ロボット１の制御装置３は、回転軸線を介して隣接する二本のリンクのなす角度を算出する角度算出部４１と、角度算出部４１により算出された角度θが所定角度以下であるか否かを監視する角度監視部４２と、を備える。【選択図】図２

Description

本発明は、人と共存して作業するロボットの制御装置に関する。

近年では、生産性向上の観点から、ロボットと作業者が同じ作業空間内で共同して作業を行うことが提案されている。そのため、安全性の観点から、そのようなロボットは、外力を検出する機能を備え、人間がロボットに接触したときに検出された外力が所定の閾値を超えると、ロボットを停止させ、それにより人間の安全を確保している。例えば特許文献１には、ロボットの動作範囲において所定エリアを設定し、エリアの内外で外力の判定条件を変更するロボットの監視装置が開示されている。

特開２０１７−７７６０８号公報

しかし、一般にロボットアームの位置や姿勢によって、実際にロボットが周囲の人間に与える影響は異なる。このため、上記従来のロボットの監視装置では、必ずしも人と共同して作業するロボットに最適とは言えないことがあった。

そこで本発明は、人と共同して作業するロボットに適した制御装置を提供することを目的とする。

本発明の一態様に係るロボットの制御装置は、一の回転軸線を介して連結された複数のリンク、および、前記回転軸線に設けられた駆動用のモータを有するロボットアームの動作を制御するロボットの制御装置であって、前記回転軸線を介して隣接する二本のリンクのなす角度を算出する角度算出部と、前記角度算出部により算出された前記角度が所定角度以下であるか否かを監視する角度監視部と、を備える。

上記構成によれば、ロボットアームの隣接する二本のリンクのなす角度（関節角度）が、周囲の障害物（例えば作業者）を挟み込む可能性のある角度（例えば２０度未満の範囲）であるか否かを監視することができるので、人と共同して作業するロボットに適した制御装置を提供できる。

また、上記ロボットの制御装置が、前記角度算出部により算出された前記角度が前記所定角度以下である場合には、前記モータ速度が所定速度を超えないように前記ロボットアームの動作を制御してもよい。

ロボットと作業者が同じ作業空間内で共同して作業する場合、ロボットの動作を速くすると作業性は向上するが、作業者の安全性の確保が問題になる場合がある。一方、ロボットの動作を遅くすると作業者の安全性は確保できるが、作業性は低下する。このようにロボットの作業性と作業者の安全性は、トレードオフの関係にある。上記構成によれば、周囲の物を挟み込む可能性のない関節角度（ロボットアームの隣接するリンクのなす角度が所定角度よりも大きい場合）ではロボットアームを可能な限り高速度で動作させることができる。一方で、周囲の障害物を挟み込む可能性のある角度（例えば２０度）以下では、モータ速度が所定速度を超えないようにロボットアームの動作を制御する。これにより、例えばロボットの近くの作業者の部位が隣接するリンクの間に挟まれた場合でも、リンクが高速で衝突することはない。従って、ロボットの作業性と作業者の安全性の両立を図ることができる。

さらに、上記ロボットの制御装置が、前記角度算出部により算出された前記角度が前記所定角度以下である場合に、前記モータ速度が前記所定速度を超えたか否かを監視する速度監視部を更に備え、前記モータ速度が前記所定速度を超えた場合は、前記ロボットアームの動作を停止させてもよい。

上記構成によれば、関節角度が周囲の障害物を挟み込む可能性のある角度以下である場合に、モータ速度が所定速度を超えた場合は、ロボットアームの動作を停止させるので、作業者の安全性が更に向上する。

尚、前記ロボットアームは、前記駆動用のモータの回転角度位置を検出する位置センサを更に有し、前記角度算出部は、前記位置センサの検出信号に基づいて、前記回転軸線を介して隣接する二本のリンクのなす角度を算出するようにしてもよい。

上記ロボットの制御装置が、前記ロボットに働く外力を検出する外力検出部と、前記角度算出部により算出された前記角度が所定角度以下である場合に、前記外力検出部によって検出された外力に基づいて、所定の監視基準で衝突を検知する力監視部と、を更に備え、衝突が検知された場合は、前記ロボットアームの動作を停止させるようにしてもよい。

上記構成によれば、周囲の物を挟み込む可能性のある角度以下では衝突を検知し、衝突を検知した後はロボットの動作を停止させることができる。作業者の安全性を向上させることができる。

上記ロボットの制御装置が、前記ロボットに働く外力を検出する外力検出部と、前記角度算出部により算出された前記角度が所定角度よりも大きい場合に、前記外力検出部によって検出された外力に基づいて、第１の監視基準で衝突を検知するとともに、前記角度算出部により算出された前記角度が所定角度以下である場合に、前記外力検出部によって検出された外力に基づいて、前記第１の監視基準とは異なる監視基準である第２の監視基準で衝突を検知する力監視部と、を更に備え、衝突が検知された場合は、前記ロボットアームの動作を停止させるようにしてもよい。

上記構成によれば、周囲の物を挟み込む可能性のない角度では第１の監視基準（例えば衝突感度を低く設定）で衝突を検知する。一方、周囲の物を挟み込む可能性のある角度以下では第２の監視基準（例えば衝突感度を高く設定）で衝突を検知する。つまり、関節角度に応じて衝突感度を変更することができるので、ロボットの作業性と作業者の安全性の両立を図ることができる。

尚、前記ロボットアームは、１又は複数の回転関節を有し、前記一の回転軸線は、前記回転関節の回転軸線であってもよい。

本発明によれば、人と共同して作業するロボットに適した制御装置を提供することができる。

図１は、第１実施形態に係るロボットの構成を概略的に示す正面図である。 図２は、図１のロボットの作業の一例を示す平面図である。 図３は、図１のロボットの全体構成を示すブロック図である。 図４は、図３の監視装置の構成を示すブロック図である。 図５は、ロボットの監視動作の一例を示すフローチャートである。 図６は、監視対象であるロボットの関節角度を模式的に示した図である。 図７は、第２実施形態に係る監視装置の構成を示すブロック図である。 図８は、ロボットの監視の動作の一例を示すフローチャートである。 図９は、図７の監視動作の変形例を示すフローチャートである。

本発明の実施の形態について、図面を参照しつつ説明する。以下では、全ての図面を通じて同一又は相当する要素には同じ符号を付して、重複する説明は省略する。

（第１実施形態）
図１は、第１実施形態に係るロボット１の構成を概略的に示す正面図である。図１に示すように、ロボット１は、台車８に固定されたベース９と、ベース９に支持された一対のロボットアーム（以下、単に「アーム」と記載する場合がある）２、２と、ベース９内に収納された、制御装置３と、を備えている。本実施形態のロボット１は同軸双腕型の水平多関節ロボットである。以下では、一対のアーム２，２を広げた方向を左右方向と称し、基軸１６の軸心に平行な方向を上下方向と称し、左右方向および上下方向に直交する方向を前後方向と称する。各アーム２は、アーム部２０と、リスト部１７と、エンドエフェクタ（図示せず）と、を備えている。なお、２本のアーム２は、実質的に同じ構造であってもよい。また、２本のアーム２は、独立して動作したり、互いに関連して動作したりすることができる。本実施形態のロボット１は、例えば、生産ラインに導入され、作業者と同じ作業空間で共同して作業を行う。

アーム部２０は、本例では、第１リンク２０ａおよび第２リンク２０ｂとで構成されている。第１リンク２０ａは、ベース９の上面に固定された基軸１６と回転関節Ｊ１により連結され、基軸１６の軸心を通る回転軸線Ａ１まわりに回動可能である。また、２本のアーム２、２の第１リンク２０ａ、２０ａの回転軸線Ａ１は同一直線上にあり、一方のアーム２の第１リンク２０ａと他方のアーム２の第１リンク２０ａとは上下に高低差を設けて配置されている。第２リンク２０ｂは、第１リンク２０ａの先端と回転関節Ｊ２により連結され、第１リンク２０ａの先端に規定された回転軸線Ａ２まわりに回動可能である。

リスト部１７は、直動関節Ｊ３及び回転関節Ｊ４を有する。リスト部１７は、直動関節Ｊ３によって、第２リンク２０ｂに対し昇降移動である。リスト部１７は、回転関節Ｊ４によって、第２リンク２０ｂに対し垂直な回転軸線Ａ３まわりに回動可能である。リスト部１７の先端には、メカニカルインターフェース１８が取り付けられる。メカニカルインターフェース１８は、直動関節Ｊ３及び回転関節Ｊ４を介して、第２リンク２０ｂの先端と連結されている。メカニカルインターフェース１８には作業用のエンドエフェクタ（図示せず）が取り付けられる。

上記構成の各アーム２は、各関節Ｊ１〜Ｊ４を有する。各関節Ｊ１〜Ｊ４は、例えばサーボ機構（図示しない）により駆動される。サーボ機構は、アーム２を変位駆動するための駆動部と、駆動部の動力をアーム２に伝達するための伝達機構とを含む。本実施の形態では、駆動部は、例えばサーボモータによって実現され、各サーボモータにはモータの回転角度位置を検出するエンコーダ等の位置センサがそれぞれ設けられる（図示しない）。ここで回転角度位置とは、各サーボモータの関節座標系における各関節の角度の位置である（以下では、関節角度位置ともいう）。制御装置３は、サーボモータを位置制御することにより、左右のアーム２の動作を任意の速度で制御するように構成される。

図２は、図１のロボット１の作業の一例を示す平面図である。このロボット１は、人一人分に相当する限られたスペース（例えば６１０ｍｍ×６２０ｍｍ）に設置することができる。図２に示すように、ロボット１は、生産ラインに導入され、作業者と同じラインで共同して作業をする。作業台１００の上には、例えば、４種類のワークＷ１、Ｗ２，Ｗ３，Ｗ４が配置されている。ロボット１は、作業台１００の上で作業者と同様な作業を行うように作業スペースが与えられている。ロボット１の左右のアーム２の各々の先端（メカニカルインターフェース１８）には作業用のエンドエフェクタ１９がそれぞれ連結されている。本実施形態では、左右のエンドエフェクタ１９は、同じ構造である。作業台１００上のロボットの作業スペースの左右両側の領域は、それぞれ作業者が各自の作業を行う作業スペースである。例えば左側に位置する作業者が、ロボット１に材料部材であるワークＷ１を供給する。ロボット１は供給されたワークＷ１に対して、第１の部品であるワークＷ２及び第２の部品であるワークＷ３を取り付けて、ワークＷ４を完成させる。右側に位置する作業者は、完成されたワークＷ４に対して次の作業行程を行う。

ロボット１は基準座標系（以下、ベース座標系という）を持っている。この座標系は、例えば、ベース（基台）９の設置面と第１関節Ｊ１の回転軸線Ａ１との交点が原点であり、第１関節Ｊ１の回転軸線がＺ軸であり、Ｚ軸に直交する任意の軸がＸ軸であり、Ｚ軸及びＸ軸に直交する軸がＹ軸である。ロボット１のアーム２の可動範囲は、第１関節Ｊ１を中心とした円形領域である（図示せず）。各アーム２の動作範囲は、このベース座標系を基準として設定される。本実施形態では、動作領域は、少なくとも、ロボット１の正面に配置された作業台１００を覆うように設定される。このような場合、アーム２の位置や姿勢によっては、例えばアーム部２０の第１リンク２０ａと第２リンク２０ｂのなす角度が急峻になる場合がある。このため、ロボット１の両側で作業を行う作業者が、アーム２の回転関節Ｊ２の回転軸線Ａ２を介して連結された第１リンク２０ａと第２リンク２０ｂの間に挟まれる可能性がある。

そこで、本実施形態のロボット１の制御装置３は、周辺の作業者の安全を確保するために、自身の動作を監視する監視機能を備えている。図３は、ロボット１の全体構成を示すブロック図である。図３に示すように、ロボット１は、ロボットアーム２と、制御装置３と、監視装置４を備える。ロボットアーム２は、１以上の関節Ｊと、関節を介して連結された複数のリンクと、各関節Ｊに設けられた駆動用のサーボモータＭを備える。各サーボモータＭには、モータの位置（回転子の基準回転角度位置に対する回転角度位置）を検出するエンコーダ等の位置センサＥと、モータを駆動する電流を検出する電流センサ５が取り付けられる。

制御装置３は、ロボットアーム２とケーブルＣ（太字で図示）を介して接続される。ここでケーブルＣは関節ＪのサーボモータＭやブレーキ（図示せず）等に電源を供給するための電源ライン、サーボモータＭに取り付けられた位置センサＥからのセンサ信号を受信するための信号ライン等が含まれる。また、制御装置３は監視装置４と通信ケーブル（図示しない）を介して接続される。ここでケーブルＣは、例えばＲＳ４２２等のシリアル通信用のケーブルである。本実施形態では、制御装置３は、ケーブルＣを介して、監視装置４に監視信号（位置センサの検出信号）を供給するとともに、監視装置４から速度制限指令を受信し、これに従ってロボット１の動作速度を変更するように構成される。ここでロボット１の動作速度とは、ロボットアーム２を構成するサーボモータＭの回転速度（以下、「モータ速度」ともいう）である。

制御装置３は、演算処理器６、サーボアンプ７、メモリ、入出力インタフェース、通信インタフェース等を備えたロボットコントローラである。演算処理器６は、電流指令値生成部６１と、速度制限値設定部６２とを備える。ここで各部（６１及び６２）は、演算処理器６において、所定のプログラムが実行されることによって、実現される機能ブロックである。電流指令値生成部６１は、ロボットの動作プログラムに基づいて、関節Ｊを駆動するサーボモータＭの位置指令値を生成し、生成した位置指令値と位置センサからの検出値（実際値）の偏差に基づいて速度指令値を生成する。そして、生成した速度指令値と速度現在値の偏差に基づいてトルク指令値（電流指令値）を生成し、サーボアンプ７に出力する。サーボアンプ７は、サーボモータＭに対応して設けられ、与えられる電流指令値に基づいて電流を発生し、ケーブルＣを介して発生した電流をサーボモータＭに供給する。つまり、各サーボアンプ７は電流指令値に応じてサーボモータＭの駆動電流を発生する増幅器である。制御装置３は、位置指令値に基づいて各関節Ｊに設けられたサーボモータＭを位置制御することにより、各ロボットアーム２の動作を制御するように構成される。

速度制限値設定部６２は、監視装置４から受信した速度制限指令に基づいて、モータ速度が所定の速度を超えないように速度制限値を設定する。本実施形態では、速度制限値設定部６２は、初期値として、速度制限値を第１速度（例えば８００ｍｍ／ｓ）に設定している。電流指令値生成部６１は、速度制限値設定部６２により設定された速度制限値を超えないように、生成した速度指令値に制約を与える。

図４は、図３の監視装置４の構成を示すブロック図である。図４に示すように、監視装置４は、角度算出部４１と、角度監視部４２と、速度制限値生成部４３と、速度算出部４４と、速度監視部４５と、停止信号生成部４６と、を備える。ここで監視装置４は、１以上のプロセッサ、メモリ、入出力インタフェース、通信インタフェース等を備えたコンピュータである。各部（４１〜４６）は、プロセッサにおいて、所定のプログラムが実行されることによって、実現される機能ブロックである。

角度算出部４１は、関節Ｊを介して隣接する二本のリンクのなす角度（以下、関節角度ともいう）θを算出するように構成される。本実施形態では、角度算出部４１は、所定の関節に設けられたサーボモータＭの回転角度位置（位置センサＥの検出信号）に基づいて、関節角度θを算出する。

角度監視部４２は、角度算出部４１により算出された関節角度θが所定角度以下であるか否かを監視するように構成される。本実施形態では所定角度は２０度である。尚、監視装置４は、監視対象である関節や関節角度の所定角度の設定値を、例えば管理者により任意の値に調整可能な入力手段（図示せず）を備える。角度監視部４２は、各サーボモータの回転角度位置及び予め設定された各リンクの長さや形状等の情報に基づいて、ロボット１の３次元モデルを生成するように構成されていてもよい。

速度制限値生成部４３は、関節角度θが所定角度以下である場合には、モータ速度が所定速度を超えないように速度制限指令を生成し、制御装置３に送信するように構成される。本実施形態では、速度制限値生成部４３は、関節角度θが所定角度以下である場合には、モータ速度が第１速度（初期値）よりも低い第２速度を超えないような速度制限指令を生成し、制御装置３に送信するように構成される。

速度算出部４４は、各サーボモータＭの回転角度位置（位置センサＥの検出信号）に基づいて、各サーボモータＭの速度（モータ速度）を演算する。ここでは監視信号に含まれる各サーボモータＭの回転角度位置が、通信ケーブルを介して制御装置３から監視装置４に送信され、速度算出部４４に入力される。速度算出部４４は、算出結果を速度監視部４５に出力するように構成されている。

速度監視部４５は、モータ速度が所定の速度を超えているか否かを監視する。本実施形態では、速度監視部４５は、角度算出部４１により算出された関節角度θが所定角度以下である場合に、各ロボットアーム２の関節Ｊ１〜Ｊ４に設けられたサーボモータＭのうち、いずれかのモータ速度が第２速度を超えているか否かを監視する。

停止信号生成部４６は、関節角度θが所定角度以下である場合に、速度監視部４５によりモータ速度が第２速度を超えていると判定された場合にはロボット１の停止信号を生成し、これを制御装置３に供給する。

本実施形態の監視装置４は、関節角度監視機能に加え速度監視機能を備えている。

次に、監視装置４によるロボット１の監視動作について図５のフローチャートを参照しつつ説明する。図５に示すように、まず、監視装置４は、制御装置３から所定期間ごとに送信される監視信号の受信を待機する（図５のステップＳ１１）。本実施形態では、監視信号は、ロボットアーム２の位置制御において使用される各関節Ｊに設けられた関節角度位置を検出する位置センサＥの検出信号を含む。

次に、監視装置４は、監視信号を受信したときは、角度算出部４１により関節角度θを算出する（図５のステップＳ１２）。図６は、監視対象であるロボット１の関節角度θを模式的に示した図である。図６に示すように、監視対象である関節角度θは、関節Ｊ２の回転軸線Ａ２を介して隣接する第１リンク２０ａ及び第２リンク２０ｂの長手方向のなす関節角度である。ここではベース座標系で定義されたＸＹ平面における第１関節Ｊ１乃至第４関節Ｊ４がそれぞれ示されている。尚、ＸＹ平面における直動関節Ｊ３と回転関節Ｊ４の中心位置座標は同一であるので、同図では回転関節Ｊ４のみ示している。Ｌ１及びＬ２は、第１リンク２０ａおよび第２リンク２０ｂの長手方向の長さである。これらの情報は予め監視装置４のメモリに記憶されている。θ１およびθ２は、第１関節Ｊ１の回転角度位置及び第２関節Ｊ２の回転角度位置である。これらの情報は、監視信号に含まれる。これにより、角度算出部４１は、第２関節Ｊ２の回転角度位置（θ２）に相当する位置センサＥの検出信号に基づいて、関節Ｊ２を介して隣接する第１リンク２０ａ及び第２リンク２０ｂのなす関節角度θを次式（１）から算出することができる。

θ＝１８０°−θ２・・・（１）
次に、角度監視部４２は、角度算出部４１により算出された関節角度θが所定角度（２０度）以下であるか否かを監視する（図５のステップＳ１３）。所定角度は、第１リンク２０ａ及び第２リンク２０ｂによって周囲の障害物（例えば作業者）を挟み込む可能性のある所定の角度である。本実施形態では所定角度は２０度であるが、ロボット１の周囲の状況や想定される障害物の大きさによって適宜設定されてもよい。角度監視部４２は、関節角度θが２０度よりも大きく、且つ、１８０度以下の範囲である場合には第１リンク２０ａ及び第２リンク２０ｂによってロボット１の周囲に存在する障害物を挟み込む可能性はないと判定する。一方、角度監視部４２は、関節角度θが２０度以下であり、且つ、０度以上の範囲である場合には第１リンク２０ａ及び第２リンク２０ｂによってロボット１の周囲に存在する障害物を挟み込む可能性があると判定する。

次に、速度制限値生成部４３は、角度算出部４１により算出された関節角度θが２０度以下である場合には（図５のステップＳ１３でＹＥＳ）、モータ速度が第１速度よりも低い第２速度を超えないような速度制限指令を生成し、これを制御装置３に供給する（図５のステップＳ１４）。ここで第２速度は、ＩＳＯ１０２１８−１に低速制御として規定されている２５０ｍｍ／ｓである。これにより、制御装置３は、モータ速度が第２速度を超えないようにロボットアーム２の動作を制御する。例えばロボットの近くの作業者の部位が隣接するリンクの間に挟まれた場合でも、リンクが高速で衝突することはない。

次に、速度監視部４５は、角度算出部４１により算出された関節角度θが２０度以下である場合に、モータ速度が第２速度を超えているか否かを監視する（図５のステップＳ１５）。本実施形態では、速度監視部４５は、関節角度θが２０度以下である場合に、各ロボットアーム２の関節Ｊ１〜Ｊ４に設けられたサーボモータＭのうち、いずれかのモータ速度が第１速度を超えているか否かを監視する。

次に、停止信号生成部４６は、関節角度θが２０度以下である場合には（図５のステップＳ１５でＹＥＳ）、速度監視部４５によりモータ速度が第２速度を超えていると判定された場合にはロボット１の停止信号を生成し、これを制御装置３に供給する（図５のステップＳ１６）。制御装置３はロボット１の動作を停止させる。

一方、速度制限値生成部４３は、関節角度θが２０度を超えた場合には（図５のステップＳ１３でＮＯ）、速度制限指令を生成しない。ここで速度制限値の初期値は、第１速度（例えば８００ｍｍ／ｓ）に設定されているので、制御装置３は、モータ速度が第１速度を超えないようにロボットアーム２の動作を高速で制御する。これにより、ロボット１の能力を最大限発揮させることができる。制御装置３は、以上のような動作をロボット１の作業が終了するまで繰り返し行う。

一般に、図２に示すように、ロボット１と作業者が同じ作業空間内で共同して作業する場合、ロボット１の動作を速くすると作業性は向上するが、作業者の安全性の確保が問題になる場合がある。一方、ロボット１の動作を遅くすると作業者の安全性は確保できるが、作業性は低下する。このようにロボット１の作業性と作業者の安全性は、トレードオフの関係にある。そこで、本実施形態によれば、ロボットアーム２の関節角度θが２０度よりも大きい場合（図５のステップＳ１３でＮＯ）には周囲の障害物（例えば作業者）を挟み込む可能性が無いと判断しロボットアーム２を可能な限り高速度（第１速度）で動作させる。一方で、関節角度θが２０度以下では、周囲の障害物（例えば作業者）を挟み込む可能性が有ると判断しロボットアーム２を低速度（第２速度）で動作させる。これにより、例えばロボットの近くの作業者の部位が隣接するリンクの間に挟まれた場合でも、リンクが高速で衝突することはない。従って、ロボットの作業性と作業者の安全性の両立を図ることができる。

さらに、本実施形態では、関節角度θが２０度以下である場合に、ロボットアーム２の動作速度が第２速度を超えた場合は、ロボットアーム２の動作を停止させるので、作業者の安全性が更に向上する。人と共同して作業するロボットに適した制御装置３を提供することができる。

尚、本実施形態では、角度算出部４１は、第２関節Ｊ２の回転角度位置（θ２）に相当する位置センサＥの検出信号に基づいて、関節角度θを算出したが、これに限られない。角度算出部４１は、第１リンク２０ａと第２リンク２０ｂの当該関節Ｊ２側とは反対側における各先端の位置の相対距離Ｌに基づいて、関節角度θを算出するようにしてもよい。ここで相対距離Ｌは、図６に示すように、第１関節Ｊ１の中心位置座標であるベース座標系の原点Ｇ（０，０）から第４関節Ｊ４の中心Ｐの位置座標（ｘ，ｙ）との間の距離である。例えば、公知の３次元カメラを用いることにより、３次元空間上の２点間の距離を計測することにより相対距離Ｌの算出することができる。

これにより、関節Ｊ１，Ｊ２及びＪ４を頂点とする三角形の３辺の長さが定まったので、次式（２）の余弦定理を用いて関節角度θを算出することができる。

ｃｏｓθ＝（Ｌ１^２＋Ｌ２^２−Ｌ^２）／２×Ｌ１×Ｌ２・・・（２）

（第２実施形態）
次に、第２実施形態について説明する。以下では、第１実施形態と共通する構成の説明は省略し、相違する構成についてのみ説明する。

図７は、第２実施形態に係る監視装置の構成を示すブロック図である。図７に示すように、本実施形態では、第１実施形態（図４）と比較すると、監視装置４Ａが、速度制限値生成部４３、速度算出部４４及び速度監視部４５の代わりに、駆動トルク推定部４７、外力検出部４８及び力監視部４９を備える点が異なる。本実施形態の監視装置４Ａは、関節角度監視機能に加え力監視機能を備えている。このため、本実施形態では、制御装置３から監視装置４に送信される監視信号は、位置センサＥの検出信号、及び、電流センサ５で検出されたセンサ電流値を含む。

駆動トルク推定部４７は、位置センサＥにより算出された関節角度位置から、ロボット１の関節のサーボモータを駆動するのに必要な駆動トルクを推定する。駆動トルク推定部４７は、本実施形態では、重力トルク、慣性力トルク、及び摩擦力トルクをそれぞれ算出し、これらを加算することにより、駆動トルクの推定値を算出する。ここで重力トルクは各リンクの重量に打ち勝って姿勢を維持するためのトルクである。慣性力トルクはリンクの慣性に打ち勝つために必要なトルクである。摩擦力トルクは減速機の摩擦に打ち勝つために必要なトルクである。尚、本実施形態では、駆動トルク推定部４７は監視装置４に実装されるような構成であるが、制御装置３に実装されてもよい。駆動トルク推定値は、電流センサ５で検出されたセンサ電流値及び位置センサＥで検出された関節軸角度位置を含むセンサ信号とともに、監視信号として制御装置３から監視装置４に送信されてもよい。

外力検出部４８は、ロボット１に働く外力を検出する。本実施形態では外力検出部４８は、電流センサ５で検出された各サーボモータＭを流れるセンサ電流値をトルク値に変換する。そして、センサ電流値から変換されたトルク値から、駆動トルク推定部４７から入力された駆動トルクの推定値を減算し、これを外乱トルクとして算出する。そして、この外乱トルク値を用いてロボット１に働く外力を算出し、これを力監視部４９に出力する。具体的には、外力検出部４８は、外乱トルクτ_ｄからロボット１の先端（例えばツールセンターポイント）に働く外力ｆ_ｄを、仮想仕事の原理によって次式（３）のように求める。

ｆ_ｄ＝（Ｋ^Ｔ）^−１τ_ｄ・・・（３）
ここでＫはヤコビ行列であり、ロボット１のベース座標系と関節座標系との間の微小変位関係を表現した行列である。ヤコビ行列Ｋについて、誤差Δｘと関節角差分Δθには式（４）の関係が成立している。

Δｘ＝ＫΔθ・・・・・・（４）
このように外力検出部４８は、式（３）のように外乱トルクτ_ｄにヤコビ行列Ｋの転置行列Ｊ^Ｔの逆行列を乗じることによりロボット１に働く外力ｆ_ｄを算出し、これを力監視部４９に出力する。尚、式（３）の外力ｆ_ｄはロボット１の先端で作用していると想定したときの外力である。外力ｆ_ｄがロボット１の先端以外を作用点としている場合は、外力ｆ_ｄを実際の作用点での外力に座標変換してもよい。

力監視部４９は、角度算出部４１により算出された関節角度θが所定角度よりも大きい場合、外力検出部４８によって検出された外力に基づいて、第１の監視基準で衝突を検知するように構成される。具体的には、力監視部４９は、外力検出部４８から入力された外力の値ｆ_ｄの微分値に比例した値ｆ’_ｄをロボット１に働く衝撃力として算出し、ロボット１に働く衝撃力の値｜ｆ’_ｄ｜が予め設定された第１閾値ｆ_ｔｈ１を超えたか否かを判定し、第１閾値ｆ_ｔｈ１を超えたときにロボット１が衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力するように構成されている。第１閾値ｆ_ｔｈ１との比較対象である衝撃力の値｜ｆ’_ｄ｜は外力ｆ’_ｄのスカラ値である。尚、力監視部４９は、外力検出部４８から入力された外力の値｜ｆ_ｄ｜が予め設定された閾値を超えたか否かを判定して衝突を検知してもよい。尚、監視装置４は、例えば管理者により、衝突検知の際の閾値を任意の値に調整可能な入力手段（図示せず）を備える。

停止信号生成部４６は、力監視部４９から衝突検知信号が入力された場合にはロボット１の停止信号を生成し、これを制御装置３に出力する。

次に、監視装置４Ａによるロボット１の監視動作について図８のフローチャートを参照しつつ説明する。図８のステップＳ２１からステップＳ２３までの角度算出部４１及び角度監視部４２の動作は、図５のステップＳ１１からステップＳ１３までの動作と同じであるので、説明を省略する。

図８に示すように、力監視部４９は、関節角度θが所定角度以下である場合（図８のステップＳ２３でＹＥＳ）、外力検出部４８によって検出された外力に基づいて、第１の監視基準で衝突を検知する（図８のステップＳ２４）。具体的には、力監視部４９は、外力検出部４８から入力された外力の値｜ｆ_ｄ｜が予め設定された第１閾値ｆ_ｔｈ１を超えたか否かを判定し、第１閾値ｆ_ｔｈ１を超えたときにロボット１が衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力する。このように、力監視部４９は、角度監視部４２により第１リンク２０ａ及び第２リンク２０ｂによってロボット１の周囲に存在する障害物（例えば作業者）を挟み込む可能性があると判断された場合、衝突を検知する。

そして、停止信号生成部４６は、力監視部４９から衝突検知信号が入力された場合にはロボット１の停止信号を生成し（図８のステップＳ２５）、これを制御装置３に出力する。これにより、制御装置３は、ロボット１の動作を停止させることができる。

本実施形態によれば、周囲の物を挟み込む可能性の有る角度では、衝突を検知し、衝突を検知した後はロボットの動作を停止させることができるので、作業者の安全性を向上させることができる。これにより、人と共同して作業するロボットに適した制御装置３を提供することができる。

＜変形例＞
次に、本実施形態の力監視機能の変形例について説明する。本変形例では、関節角度に応じて２つの異なる監視基準で衝突を検知する。具体的には、図７の力監視部４９は、角度算出部４１により算出された関節角度θが所定角度よりも大きい場合、外力検出部４８によって検出された外力に基づいて、第１の監視基準で衝突を検知するように構成される。具体的には、力監視部４９は、外力検出部４８から入力された外力の値｜ｆ_ｄ｜が予め設定された第１閾値ｆ_ｔｈ１を超えたか否かを判定し、第１閾値ｆ_ｔｈ１を超えたときにロボット１が衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力するように構成されている。本実施形態では第１閾値ｆ_ｔｈ１は１００Ｎに設定される。第１閾値ｆ_ｔｈ１との比較対象である外力の値｜ｆ_ｄ｜は外力ｆ_ｄのスカラ値である。

一方、力監視部４９は、力監視部４９は角度算出部４１により算出された関節角度θが所定角度以下である場合、外力検出部４８によって検出された外力に基づいて、第１の監視基準とは異なる監視基準である第２の監視基準で衝突を検知するように構成される。具体的には、力監視部４９は、外力検出部４８から入力された外力の値ｆ_ｄの微分値に比例した値ｆ’_ｄをロボット１に働く衝撃力として算出し、ロボット１に働く衝撃力の値｜ｆ’_ｄ｜が予め設定された第２閾値ｆ_ｔｈ２超えたか否かを判定し、第２閾値ｆ_ｔｈ２を超えたときにロボット１が衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力するように構成されている。第２閾値ｆ_ｔｈ２との比較対象である衝撃力の値｜ｆ’_ｄ｜は外力ｆ’_ｄのスカラ値である。

図９は、本変形例の監視動作を示すフローチャートである。図９に示すように、力監視部４９は、関節角度θが２０度を超えた場合には（図９のステップＳ２３でＮＯ）、外力検出部４８によって検出された外力に基づいて、第１の監視基準で衝突を検知する（図９のステップＳ２４−１）。具体的には、力監視部４９は、外力検出部４８から入力された外力の値｜ｆ_ｄ｜が予め設定された第１閾値ｆ_ｔｈ１を超えたか否かを判定し、第１閾値ｆ_ｔｈ１を超えたときにロボット１が衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力する。このように、力監視部４９は、角度監視部４２により第１リンク２０ａ及び第２リンク２０ｂによってロボット１の周囲に存在する障害物（例えば作業者）を挟み込む可能性はないと判断された場合は、衝突感度を低く設定し衝突を検知する。

一方、力監視部４９は、関節角度θが所定角度以下である場合（図９のステップＳ２３でＹＥＳ）、外力検出部４８によって検出された外力に基づいて、第１の監視基準とは異なる監視基準である第２の監視基準で衝突を検知する（図９のステップＳ２４−２）。具体的には、力監視部４９は、外力検出部４８から入力された外力の値ｆ_ｄの微分値に比例した値ｆ’_ｄをロボット１に働く衝撃力として算出し、ロボット１に働く衝撃力の値｜ｆ’_ｄ｜が予め設定された第２閾値ｆ_ｔｈ２を超えたか否かを判定し、第２閾値ｆ_ｔｈ２を超えたときにロボット１が衝突したと判定して衝突検知信号を生成し、これを停止信号生成部４６に出力する。このように、力監視部４９は、角度監視部４２により第１リンク２０ａ及び第２リンク２０ｂによってロボット１の周囲に存在する障害物（例えば作業者）を挟み込む可能性があると判断された場合は、衝突感度を高く設定し衝突を検知する。

そして、停止信号生成部４６は力監視部４９から衝突検知信号が入力された場合には、ロボット１の停止信号を生成し（図９のステップＳ２５）、これを制御装置３に出力する。これにより、制御装置３は、ロボット１の動作を停止させることができる。

本変形例によれば、周囲の物を挟み込む可能性のない角度では第１の監視基準（例えば衝突感度を低く設定）で衝突を検知する。一方、周囲の物を挟み込む可能性のある角度以下では第２の監視基準（例えば衝突感度を高く設定）で衝突を検知する。つまり、関節角度に応じて衝突感度を変更することができるので、ロボットの作業性と作業者の安全性の両立を図ることができる。

尚、本実施形態では、衝突を検知した後はロボットの動作を停止させるようにしたが、周囲の作業者又は管理者に報知するようにしてもよい。

また、本実施形態の監視装置４Ａは、関節角度を監視する関節角度監視機能とともに、関節角度に応じてロボット１に働く外力に基づいて衝突検知を行う力監視機能を備えたが、第１実施形態の関節角度に応じた速度監視機能を組み合わせてもよい。

尚、本実施形態では、力センサを用いることなく、サーボモータの電流値に基づいてロボット１に働く外力を計算するので、安価且つ高精度に衝突を検知することができる。これにより、ロボットとその周辺で作業する作業者の共同作業における利便性が更に向上する。

（その他の実施形態）
尚、上記各実施形態では、関節Ｊ２の回転軸線Ａ２を介して隣接する第１リンク２０ａ及び第２リンク２０ｂのなす角度を監視したが（図２参照）、これに限られない。同軸双腕型のロボット１において、２つの関節Ｊ１に共通する回転軸線Ａ１を介して隣接する２本の第１リンク２０ａ及び第１リンク２０ａのなす角度を監視してもよい。

尚、上記各実施形態のロボット１は、双腕型の水平多関節型ロボットであったが、これに限定されることはない。単腕ロボットでもよいし、垂直多関節型ロボットでもよい。また、各ロボットアームは、少なくとも一つの関節を介して隣接する二本のリンクを備えていればこれに限定されない。

尚、上記各実施形態では、関節角度θは、関節を介して隣接する二本のリンクのなす角度と定義したが、二本のリンクのうちの一方をロボットアーム２先端のエンドエフェクタ、又は、ロボット１のベースで置き換えてもよい。つまり、関節角度θは、関節を介して隣接するリンクとロボットアーム２先端のエンドエフェクタ、又はリンクとロボットのベースのなす角度と定義してもよい。

尚、上記各実施形態の監視装置４，４Ａは制御装置３と別々に設けたが、制御装置３に含まれていてもよい。例えば制御装置３の演算処理器６において、監視装置４の各部の機能ブロックが実行されるように構成されていてもよい。

上記説明から、当業者にとっては、本発明の多くの改良や他の実施形態が明らかである。従って、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び機能の双方又は一方の詳細を実質的に変更できる。

本発明は、人と共存して作業するロボットの制御装置に有用である。

１ ロボット
２ ロボットアーム
３ 制御装置
４，４Ａ 監視装置
５ 電流センサ
６ 演算処理器
７ サーボアンプ
８ 台車
９ ベース
１７ リスト部
１８ メカニカルインターフェース
１９ エンドエフェクタ
２０ アーム部
２０ａ 第１リンク
２０ｂ 第２リンク
４１ 角度検出部
４２ 角度監視部
４３ 速度制限値生成部
４４ 速度算出部
４５ 速度監視部
４６ 停止信号生成部
４７ 駆動トルク推定部
４８ 外力検出部
４９ 力監視部
６１ 電流指令値生成部
６２ 速度制限値設定部
１００ 作業台
Ｊ 関節
Ｍ サーボモータ
Ｅ エンコーダ（位置センサ）
Ｃ ケーブル
θ 関節角度

Claims (7)

1. 一の回転軸線を介して連結された複数のリンク、および、前記回転軸線に設けられた駆動用のモータを有するロボットアームの動作を制御するロボットの制御装置であって、
   前記回転軸線を介して隣接する二本のリンクのなす角度を算出する角度算出部と、
   前記角度算出部により算出された前記角度が所定角度以下であるか否かを監視する角度監視部と、
   を備える、ロボットの制御装置。
2. 前記角度算出部により算出された前記角度が前記所定角度以下である場合には、前記モータ速度が所定速度を超えないように前記ロボットアームの動作を制御する、請求項１に記載のロボットの制御装置。
3. 前記角度算出部により算出された前記角度が前記所定角度以下である場合に、前記モータ速度が前記所定速度を超えたか否かを監視する速度監視部を更に備え、
   前記モータ速度が前記所定速度を超えた場合は、前記ロボットアームの動作を停止させる、請求項２に記載のロボットの制御装置。
4. 前記ロボットアームは、前記駆動用のモータの回転角度位置を検出する位置センサを更に有し、
   前記角度算出部は、前記位置センサの検出信号に基づいて、前記回転軸線を介して隣接する二本のリンクのなす角度を算出する、請求項１又は２に記載のロボットの制御装置。
5. 前記ロボットに働く外力を検出する外力検出部と、
   前記角度算出部により算出された前記角度が所定角度以下である場合に、前記外力検出部によって検出された外力に基づいて、所定の監視基準で衝突を検知する力監視部と、
   を更に備え、
   衝突が検知された場合は、前記ロボットアームの動作を停止させる、請求項１乃至４のいずれか一項に記載のロボットの制御装置。
6. 前記ロボットに働く外力を検出する外力検出部と、
   前記角度算出部により算出された前記角度が所定角度よりも大きい場合に、前記外力検出部によって検出された外力に基づいて、第１の監視基準で衝突を検知するとともに、前記角度算出部により算出された前記角度が所定角度以下である場合に、前記外力検出部によって検出された外力に基づいて、前記第１の監視基準とは異なる監視基準である第２の監視基準で衝突を検知する力監視部と、
   を更に備え、
   衝突が検知された場合は、前記ロボットアームの動作を停止させる、請求項１乃至４のいずれか一項に記載のロボットの制御装置。
7. 前記ロボットアームは、１又は複数の回転関節を有し、
   前記一の回転軸線は、前記回転関節の回転軸線である、請求項１乃至６のいずれか一項に記載のロボットの制御装置。

Images