WO2017073052A1

WO2017073052A1 - ロボットシステムの監視装置

Info

Publication number: WO2017073052A1
Application number: PCT/JP2016/004694
Authority: WO
Inventors: 宗藤　康治
Original assignee: 川崎重工業株式会社
Priority date: 2015-10-30
Filing date: 2016-10-25
Publication date: 2017-05-04
Also published as: US20180311836A1; JPWO2017073052A1; KR20180067652A; EP3369536A1; EP3369536B1; EP3369536A4; CN108136604A; TWI621004B; JP6924146B2; US10730191B2; TW201723711A

Abstract

１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置が提供される。ロボットシステムの監視装置は、各サーボモータに流れる電流値を検出する電流センサと、電流センサで検出された各サーボモータを流れる電流値をトルク値に変換する電流トルク変換部と、各サーボモータの駆動に必要な駆動トルクの少なくとも一部を推定する駆動トルク推定部と、電流トルク変換部で変換されたトルク値と駆動トルクの推定値との差分トルクを演算する差分トルク演算部と、差分トルク演算部で演算された前記差分トルクをロボットの外力に変換するトルク外力変換部と、トルク外力変換部で変換された外力の値に基づいてロボットの停止信号を生成し、これを制御装置に供給する停止信号生成部と、を備える。

Description

ロボットシステムの監視装置

　本発明は、ロボットシステムの監視装置に関する。

　従来、力センサを用いないで、ロボットに外力が加わったときに、外力に追従するようにロボットを制御する制御装置が知られている（特許文献１～特許文献５を参照）。この制御装置は、ロボットの関節軸を駆動するモータに流れる電流値とモータの回転速度を検出して、ロボットにかかった外乱トルクを算出し、外乱トルクに応じて位置指令値を変更する。

　近年では、生産性向上の観点から、ロボットと作業者が同じ作業空間内で共同して作業を行うことが提案されている。そのため、安全性の観点から、ロボットの動作を監視して、衝突を高精度に検出するための様々な技術が開発されている（特許文献６、特許文献７を参照）。例えば特許文献７に記載された制御装置は、検出されたモータ回転角と入力されたロボットアームの負荷の重量及び重心位置から、逆動力学演算を行うことにより、モータ駆動に必要なモータ電流を計算する。そして、このモータ電流計算値とモータから検出される電流検出値との差を、衝突により発生する外乱電流値として計算し、この外乱電流値に基づいて衝突を検出する。

特開平１０－１５６７７１号公報特開平１１－０４２５７５号公報特開平１１－０４２５７６号公報特開平１１－０４２５７７号公報特開平１１－０４２５７８号公報特開２００６－０７５９３１号公報特開２００６－１２３０１２号公報

　しかし、特許文献７のような従来の構成では、ロボットの関節軸を駆動するモータの電流値に基づいて衝突を検出するため、衝突を高精度に検出するものではなかった。

　そこで本発明は、ロボット運転時の衝突を高精度に検出可能なロボットシステムの監視装置を提供することを目的とする。

　本発明の一態様に係るロボットシステムの監視装置は、１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、各サーボモータに流れる電流値を検出する電流センサと、前記電流センサで検出された各サーボモータを流れる電流値をトルク値に変換する電流トルク変換部と、各サーボモータの駆動に必要な駆動トルクの少なくとも一部を推定する駆動トルク推定部と、前記電流トルク変換部で変換されたトルク値と前記駆動トルクの推定値との差分トルクを演算する差分トルク演算部と、前記差分トルク演算部で演算された前記差分トルクを前記ロボットの外力に変換する外力変換部と、前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、を備える。

　上記構成によれば、ロボットの各関節軸を駆動するモータに流れる電流値を検出し、検出した電流値をトルク値に変換し、変換したトルク値をロボットの外力に変換する。ロボットに働く外力を直接算出するので、ロボット運転時の衝突を高精度に検出することができる。また、外力の値に基づいて停止信号を生成するので管理者は閾値等の設定が容易である。

　本発明のその他の態様に係るロボットシステムの監視装置は、１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、各サーボモータに流れる電流値を検出する電流センサと、前記電流センサで検出された各サーボモータを流れる電流値を前記ロボットの外力の検出値に変換する電流外力変換部と、各サーボモータの駆動に必要な駆動トルクの少なくとも一部を推定する駆動トルク推定部と、前記駆動トルクの推定値を前記ロボットの外力の推定値に変換するトルク外力変換部と、前記ロボットの外力の検出値と前記ロボットの外力の推定値との差分を演算することにより前記ロボットの外力に変換する外力変換部と、前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、を備える。

　本発明のその他の態様に係るロボットシステムの監視装置は、１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、各サーボモータに流れる電流値を検出する電流センサと、各サーボモータを流れる電流値を推定する電流推定部と、各サーボモータを流れる検出電流値と推定電流値との差分電流を演算する差分電流演算部と、前記差分電流演算部で算出された差分電流を前記ロボットの外力に変換する外力変換部と、前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、を備える。

　本発明のその他の態様に係るロボットシステムの監視装置は、１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、各サーボモータに流れる電流値を検出する電流センサと、前記電流センサで検出された各サーボモータを流れる電流値をトルクの検出値に変換する第１電流トルク変換部と、各サーボモータを流れる電流値を推定する電流推定部と、前記電流推定部で推定された各サーボモータを流れる推定電流値をトルクの推定値に変換する第２電流トルク変換部と、前記トルクの検出値と前記トルクの推定値との差分トルクを演算する差分トルク演算部と、前記差分トルク演算部で算出された差分トルクを前記ロボットの外力に変換する外力変換部と、前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、を備える。

　本発明のその他の態様に係るロボットシステムの監視装置は、１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、各サーボモータに流れる電流値を検出する電流センサと、各サーボモータを流れる電流値を推定する電流推定部と、前記電流センサで検出された各サーボモータを流れる電流値を前記ロボットの外力の検出値に変換する第１電流外力変換部と、前記電流推定部で推定された各サーボモータを流れる推定電流値を前記ロボットの外力の推定値に変換する第２電流外力変換部と、前記ロボットの外力の検出値と前記ロボットの外力の推定値との差分を演算することにより前記ロボットの外力に変換する外力変換部と、前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、を備える。

　前記停止信号生成部は、前記外力変換部で変換された前記外力の値が予め設定された第１閾値を超えたときに前記ロボットの停止信号を生成してもよい。

　衝突判断の際の閾値が電流値であると、管理者は電流値からロボットに働く外力を見積もらねばならず設定が煩雑である。上記構成によれば、ロボットに働く外力の値が、予め設定した閾値を超えたか否かを判定するので、管理者は閾値設定が容易である。これにより、例えば１００Ｎというような外力値を直接閾値として設定することができる。

　前記トルク外力変換部で変換された前記外力の値を入力値とするローパスフィルタを更に備え、前記停止信号生成部は、前記ローパスフィルタの出力値が予め設定された第２閾値を超えたときに前記ロボットの停止信号を生成してもよい。

　上記構成によれば、ローパスフィルタによって、ロボットに瞬間的に作用する外力の変化に過剰に反応することなく、ノイズ成分を抑制し好適に衝突判定を行うことができる。

　上記ロボットシステムの監視装置では、トルクから前記ロボットの外力への変換、又は、電流から前記ロボットの外力への変換は、前記ロボットの所定の点（例えばロボットの先端）を外力の作用点としたヤコビ行列を用いて演算されてもよい。

　本発明によれば、ロボット運転時の衝突を高精度に検出可能なロボットシステムの監視装置を提供することができる。

図１は、本発明の第１実施形態に係る監視装置が実装されたロボットシステムの全体構成を示すブロックダイアグラムである。図２は、図１の監視装置の構成を示すブロックダイアグラムである。図３は、制御装置の処理の流れを示すフローチャートである。図４は、監視装置の処理の流れを示すフローチャートである。図５は、本発明の第２実施形態に係る監視装置の構成を示すブロックダイアグラムである。図６は、本発明の第３実施形態に係る監視装置の構成を示すブロックダイアグラムである。図７は、本発明の第４実施形態に係る監視装置の構成を示すブロックダイアグラムである。図８は、本発明の第５実施形態に係る監視装置の構成を示すブロックダイアグラムである。

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

　［ロボットシステム］
　図１は、本発明の第１実施形態に係る監視装置が実装されたロボットシステムの全体構成を示すブロックダイアグラムである。図１に示すように、ロボットシステム１は、ロボット２と、制御装置３と、監視装置４を備える。ロボットシステム１は、ロボット２と作業者が同じ作業空間内で共同して作業を行うためのものである。このため、本実施形態のロボットシステム１は、作業者の安全性を向上させるためにロボット２の監視装置４を備える。

　ロボット２は、１以上の関節軸と、その関節軸を駆動させるサーボモータを備える。ロボット２は、本実施形態では、６つの関節軸Ｊ１～Ｊ６と、それらの関節軸を駆動させる６つのサーボモータＭ１～Ｍ６備える。ロボット２は、いわゆる６軸多関節ロボットである。各サーボモータＭ１～Ｍ６にはモータを駆動する電流を検出する電流センサ５、サーボモータＭ１～Ｍ６の回転を制動するブレーキ（図示せず）、及び、サーボモータＭ１～Ｍ６のモータの回転角度位置を検出するエンコーダ等の位置センサ（図示せず）がそれぞれ設けられる。ここでモータの回転角度位置とは、サーボモータＭ１～Ｍ６の関節座標系における関節軸Ｊ１～Ｊ６の角度の位置である（以下では、関節軸角度位置ともいう）。

　制御装置３は、ロボット２とケーブルＬ１～Ｌ６（太字で図示）を介して接続される。ここでケーブルＬ１～Ｌ６は関節軸Ｊ１～Ｊ６のサーボモータＭ１～Ｍ６やブレーキ等に電源を供給するための電源ライン、サーボモータＭ１～Ｍ６に取り付けられた位置センサ（図示せず）からのセンサ信号を受信するための信号ライン等が含まれる。制御装置３は、位置指令値に基づいて各関節軸Ｊ１～Ｊ６を駆動するサーボモータＭ１～Ｍ６を制御するように構成される。

　また、制御装置３は監視装置４と通信ケーブル（図示しない）を介して接続される。ここで通信ケーブルは、例えばＲＳ４２２等のシリアル通信用のケーブルである。制御装置３は、通信ケーブルを介して、監視装置４に監視信号を供給するとともに、監視装置４から停止信号を受信したときにはロボット２の動作を停止させる停止機能を備える。

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

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

　監視装置４は、ロボットシステム１においてロボット２の動作を監視して衝突を検出するように構成される。監視装置４は、制御装置３からロボット２の監視信号（センサ電流値、駆動トルク推定値）を受信し、衝突を検出した場合には、制御装置３に停止信号を供給する。監視装置４は、ロボット２とともに作業を行う作業者の安全性を向上させるために、制御装置３から独立して設けられる。例えば制御装置３と監視装置４は一つの筐体の中に収容される。

　［監視装置］
　次に、監視装置４の具体的な構成について図２のブロックダイアグラムを用いて説明する。図２に示すように、監視装置４は、電流トルク変換部４１と、差分トルク演算部４２と、外力変換部４３と、停止信号生成部４４を備える。ここで監視装置４は、１以上のプロセッサ、メモリ、入出力インタフェース、通信インタフェース等を備えたコンピュータである。ここで監視装置４は、管理者により、衝突検知の際の閾値を任意の値に調整可能な構成を備える。各部（４１～４４）は、プロセッサにおいて、所定のプログラムが実行されることによって、実現される機能ブロックである。

　電流トルク変換部４１は、電流センサ５で検出された各サーボモータＭ１～Ｍ６を流れるセンサ電流値をトルク値に変換する。電流センサ５で検出されたセンサ電流値は監視信号として、通信ケーブルを介して制御装置３から監視装置４に送信され、電流トルク変換部４１に入力される。電流トルク変換部４１は、入力されたセンサ電流値をトルク値に変換し、これを差分トルク演算部４２に出力する。

　差分トルク演算部４２は、電流トルク変換部４１で変換されたトルク値と駆動トルクの推定値との差を差分トルクとして演算する。ここで駆動トルク推定値は、駆動トルク推定部６２で演算された後に、監視信号として、通信ケーブルを介して制御装置３から監視装置４に送信され、差分トルク演算部４２に入力される。尚、本実施形態では、制御装置３が駆動トルク推定部６２を備えたが、監視装置４が駆動トルク推定部６２を備えてもよい。差分トルク演算部４２は、本実施形態では、加減算器５１を備える。加減算器５１は、電流トルク変換部４１から入力される各サーボモータＭ１～Ｍ６のトルク値から、駆動トルク推定部６２から入力される駆動トルク推定値を減算して差分トルクを算出し、これを外力変換部４３に出力する。

　外力変換部４３は、差分トルク演算部４２で演算された差分トルク値をロボット２の外力に変換し、これを停止信号生成部４４に出力する。

　停止信号生成部４４は、外力変換部４３で変換された外力のスカラ値に基づいてロボット２の停止信号を生成し、これを制御装置３に供給する。停止信号生成部４４は、本実施形態では、第１衝突判定部５２と、ローパスフィルタ５３と、第２衝突判定部５４、を備える。

　第１衝突判定部５２は、外力変換部４３から入力された外力の値｜ｆ_ｄ｜が予め設定された第１閾値ｆ_ｔｈ１を超えたか否かを判定し、第１閾値ｆ_ｔｈ１を超えたときにロボット２が衝突したとして第１停止信号を生成し、これを制御装置３に出力するように構成されている。ここで第１閾値ｆ_ｔｈ１は、本実施形態では１００Ｎに設定される。第１閾値ｆ_ｔｈ１との比較対象である外力の値｜ｆ_ｄ｜は外力ｆ_ｄのスカラ値である。

　ローパスフィルタ５３は、外力変換部４３で変換された外力の値ｆ_ｄを入力値として、フィルタ演算を施し、これを第２衝突判定部５４に出力するように構成されている。

　第２衝突判定部５４は、ローパスフィルタ５３の出力値が予め設定された第２閾値ｆ_ｔｈ２を超えたか否かを判定し、第２閾値ｆ_ｔｈ２を超えたときにロボット２の第２停止信号を生成し、これを制御装置３に出力するように構成されている。ここで第２閾値ｆ_ｔｈ２は、本実施形態では８０Ｎに設定される。第２閾値ｆ_ｔｈ２との比較対象であるローパスフィルタ５３の出力値もスカラ値である。

　［ロボットの動作］
　次に、以上のような構成のロボットシステム１におけるロボット２の動作について図１を用いて説明する。

　制御装置３は、予め定められた位置指令値と位置センサからの関節軸角度位置情報に基づいて電流指令値を演算する。サーボアンプ７は、電流指令値に基づいて電流を発生し、ケーブルＬ１～Ｌ６を介して発生した電流をサーボモータＭ１～Ｍ６に供給する。サーボモータＭ１～Ｍ６に電流が流れると、各関節軸Ｊ１～Ｊ６の関節軸角度が変位してロボット２の手先の位置が目的の位置へ移動する。このように制御装置３は、位置指令値に基づいて各関節軸Ｊ１～Ｊ６に設けられたサーボモータＭ１～Ｍ６を位置制御する。

　［ロボットの監視］
　次に、ロボットシステム１におけるロボット２の動作監視について図３及び図４のフローチャートも参照しつつ説明する。図３に示すように、まず、制御装置３は、ロボット２の動作を監視するために必要な監視信号を生成する（図３のステップＳ３１）。制御装置３は、監視信号として、電流センサ５によりセンサ電流値を検出するとともに、駆動トルク推定部６２により駆動トルク推定値を推定する（図１参照）。駆動トルク推定部６２は、位置センサにより算出された関節軸角度位置から例えば時間差分を取ることにより駆動軸速度を算出する。そして、予めメモリに記憶された摩擦係数を読み出して、算出された駆動軸速度と摩擦係数からクーロン摩擦、粘性摩擦等に相当する摩擦力トルクを算出する。駆動トルク推定部６２は、位置センサにより算出された関節軸角度位置から関節角速度を算出する。そして、算出された関節角速度から関節角加速度を算出する。予めメモリに記憶されたリンクパラメータを読み出し、リンクパラメータ及び位置センサにより算出された関節軸角度位置から各リンクの慣性モーメントを算出する。算出された関節角加速度、及び各リンクの慣性モーメントから慣性力トルクを算出する。駆動トルク推定部６２は、予めメモリに記憶されたリンクパラメータを読み出し、リンクパラメータを用いて、位置センサにより算出された関節軸角度位置から各リンクに作用する重力を算出し、この重力を補償する重力トルクを算出する。駆動トルク推定部６２は、摩擦力トルク、慣性力トルク及び重力トルクを加算して駆動トルクの推定値を算出する。

　次に、制御装置３は、ステップＳ１で生成した監視信号を所定期間ごとに監視装置４に送信する（図３のステップＳ３２）。ここで監視信号には、電流センサ５で検出された各サーボモータＭ１～Ｍ６を流れるセンサ電流値と、駆動トルク推定部６２で演算された各サーボモータＭ１～Ｍ６を駆動するのに必要な駆動トルク推定値が含まれる。

　一方、監視装置４は、図４に示すように、制御装置３から所定期間ごとに送信される監視信号を受信する（図４のステップＳ４１）。

　監視装置４は、監視信号を受信したときは、電流トルク変換部４１により、電流センサ５で検出された各サーボモータＭ１～Ｍ６を流れるセンサ電流値をトルク値に変換する（図４のステップＳ４２）。電流トルク変換部４１は、入力されたセンサ電流値をトルク値に変換し、これを差分トルク演算部４２に出力する。

　次に、差分トルク演算部４２は、電流トルク変換部４１で変換されたトルク値と駆動トルクの推定値との差を差分トルクとして演算する（図４のステップＳ４３）。加減算器５１は、電流トルク変換部４１から入力される各サーボモータＭ１～Ｍ６のトルク値から、駆動トルク推定部６２から入力される駆動トルク推定値を減算して差分トルクを算出し、これを外力変換部４３に出力する（図２参照）。

　次に、外力変換部４３は、差分トルク演算部４２で演算された差分トルク値をロボット２の外力に変換する（図４のステップＳ４４）。ここでは外力変換部４３は、差分トルク演算部４２から入力された差分トルクτ_ｄからロボット２の先端に働く外力ｆ_ｄを、仮想仕事の原理によって次式（１）のように求める。
　　　　　　ｆ_ｄ＝（Ｊ^Ｔ）^－１τ_ｄ・・・（１）

　ここでＪはヤコビ行列であり、ロボットの座標系と関節座標系との間の微小変位関係を表現した行列である。ヤコビ行列Ｊについて、誤差Δｘと関節角差分Δθには式（２）の関係が成立している。
　　　　　　Δｘ＝ＪΔθ・・・・・・（２）

　このように外力変換部４３は、式（１）のように差分トルクτ_ｄにヤコビ行列Ｊの転置行列Ｊ^Ｔの逆行列を乗じることによりロボット２の外力ｆ_ｄに変換し、これを停止信号生成部４４に出力する。ここで、式（１）の外力ｆ_ｄはロボット２の先端で作用していると想定したときの外力である。外力ｆ_ｄがロボット２の先端以外を作用点としている場合は、外力ｆ_ｄを実際の作用点での外力に座標変換してもよい。

　次に、停止信号生成部４４は、外力変換部４３で変換された外力の値に基づいてロボット２が衝突したか否かを検出する（図４のステップＳ４５）。具体的には、図２の第１衝突判定部５２が、外力変換部４３から入力された外力の値｜ｆ_ｄ｜が予め設定された第１閾値ｆ_ｔｈ１を超えたか否かを判定し、第１閾値ｆ_ｔｈ１を超えたときにロボット２が衝突したとして第１停止信号を生成し、これを制御装置３に出力する（図４のステップＳ４６）。第１閾値ｆ_ｔｈ１との比較対象である外力の値｜ｆ_ｄ｜は外力ｆ_ｄのスカラ値である。本実施形態では、図２の第２衝突判定部５４も、ローパスフィルタ５３の出力値が予め設定された第２閾値ｆ_ｔｈ２を超えたか否かを判定し、第２閾値ｆ_ｔｈ２を超えたときにロボット２の第２停止信号を生成し、これを制御装置３に出力する（図４のステップＳ４６）。第２閾値ｆ_ｔｈ２との比較対象であるローパスフィルタ５３の出力値もスカラ値である。ローパスフィルタ５３によって、ロボット２に瞬間的に作用する外力の値ｆ_ｄの変化に過剰に反応することなく、ノイズ成分を抑制し好適に衝突判定を行うことができる。

　ここで第１閾値ｆ_ｔｈ１（１００Ｎ）及び第２閾値ｆ_ｔｈ２（８０Ｎ）は、管理者により、予め任意の値に設定される。衝突判断の際の閾値が電流値であると、管理者は電流値からロボット２に働く外力を見積もらねばならず設定が煩雑であるが、ロボットに働く外力の値が、予め設定した閾値を超えたか否かを判定するので、管理者は閾値設定が容易である。例えば１００Ｎというような外力値を直接閾値として設定することができる。

　一方、制御装置３は、図３に示すように、監視装置４から停止信号を受信したとき（図３のステップＳ３３でＹＥＳ）にはロボット２の動作を停止させる。ここでロボット２の停止態様は任意である。例えば動力を遮断することにより即座に停止させてもよいし（いわゆる非常停止）、動力を遮断し且つ減速して停止させてもよいし（いわゆる減速停止）、動力を遮断することなく、減速して停止させてもよい（いわゆる一時停止）。

　従って、本実施形態によれば、監視装置４において、ロボット２の各関節軸を駆動するモータに流れる電流値を検出し、検出した電流値をトルク値に変換し、変換したトルク値をロボットの外力に変換する。つまり、ロボット２に働く外力ｆ_ｄを直接算出するので、ロボット運転時の衝突を高精度に検出することができる。制御装置３において、ロボット２を安全に停止することができる。

　尚、本実施形態では、駆動トルク推定部６２は、重力トルク、慣性力トルク、及び摩擦力トルクをそれぞれ算出し、これらを加算することにより、駆動トルクの推定値を算出したが、これらの重力トルク、慣性力トルク、及び摩擦力トルクのうちの少なくとも一つを駆動トルクの推定値としてもよい。

　以下、本発明の第２乃至第５実施形態について説明する。以下では、第１実施形態と共通する構成の説明は省略し、相違する構成を中心に説明する。

　図５は、本発明の第２実施形態に係る監視装置の構成を示すブロックダイアグラムである。図５に示すように、監視装置４Ａは、電流センサ５で検出された各サーボモータを流れる電流値をロボット２の外力の検出値に変換する電流外力変換部４５と、駆動トルクの推定値をロボット２の外力の推定値に変換するトルク外力変換部４６とを備え、外力変換部４３がロボット２の外力の検出値とロボット２の外力の推定値との差分を演算することによりロボット２の外力に変換する点が第１実施形態（図２参照）と異なる。

　尚、電流外力変換部４５による電流ｉからロボット２の外力ｆへの変換は、ロボットの所定の点（例えばロボット２の先端）を外力の作用点としたヤコビ行列Ｊを用いて次式（３）のように演算される。ここでＸは電流ｉからトルクτへの変換を表している。
　　　　　　ｆ＝（Ｊ^Ｔ）^－１Ｘｉ・・・（３）

　トルク外力変換部４６によるトルクτからロボット２の外力ｆへの変換はロボットの所定の点（例えばロボット２の先端）を外力の作用点としたヤコビ行列Ｊを用いて演算される（式（１）参照）。

　停止信号生成部４４は、外力変換部４３（加減算器５５）で変換された外力の値に基づいてロボットの第１停止信号及び第２停止信号を生成し、これを制御装置３に供給する。

　図６は、本発明の第３実施形態に係る監視装置の構成を示すブロックダイアグラムである。図６に示すように、制御装置３が各サーボモータを流れる電流値を推定する電流推定部６３を備え、監視装置４Ｂが、電流センサ５で検出された各サーボモータを流れる検出電流値と、各サーボモータを流れる推定電流値との差分電流を演算する差分電流演算部４７を備え、外力変換部４３が、差分電流演算部４７（加減算器５６）で算出された差分電流をロボット２の外力に変換する点が第１実施形態（図２参照）と異なる。

　尚、外力変換部４３による電流ｉからロボット２の外力ｆへの変換は、ロボットの所定の点（例えばロボット２の先端）を外力の作用点としたヤコビ行列Ｊを用いて上記式（３）のように演算される。停止信号生成部４４は、外力変換部４３で変換された外力の値に基づいてロボット２の第１停止信号及び第２停止信号を生成し、これを制御装置３に供給する。

　図７は、本発明の第４実施形態に係る監視装置の構成を示すブロックダイアグラムである。図７に示すように、制御装置３が各サーボモータを流れる電流値を推定する電流推定部６３を備え、監視装置４Ｃが、電流センサ５で検出された各サーボモータを流れる電流値をトルクの検出値に変換する電流トルク変換部４１Ａと、各サーボモータを流れる推定電流値をトルクの推定値に変換する電流トルク変換部４１Ｂを備える点が第１実施形態（図２参照）と異なる。

　尚、電流トルク変換部４１Ａ，４１Ｂによる電流ｉからトルクτへの変換は次式（４）で演算される。ここでＸは電流ｉからトルクτへの変換を表している。
　　　　　　τ＝Ｘｉ・・・（４）

外力変換部４３によるトルクτからロボット２の外力ｆへの変換は、ロボットの所定の点（例えばロボット２の先端）を外力の作用点としたヤコビ行列を用いて演算される（式（１）参照）。

　図８は、本発明の第５実施形態に係る監視装置の構成を示すブロックダイアグラムである。図８に示すように、制御装置３が各サーボモータを流れる電流値を推定する電流推定部６３を備え、監視装置４Ｄが電流センサ５で検出された各サーボモータを流れる電流値をロボット２の外力の検出値に変換する電流外力変換部４５Ａと、電流推定部６３で推定された各サーボモータを流れる推定電流値をロボット２の外力の推定値に変換する電流外力変換部４５Ｂと、を備え、外力変換部４３がロボットの外力の検出値とロボットの外力の推定値との差分を演算することによりロボットの外力に変換する点が第１実施形態（図２参照）と異なる。

　尚、電流外力変換部４５Ａ及び４５Ｂによる電流ｉからロボット２の外力ｆへの変換は、ロボットの所定の点（例えばロボット２の先端）を外力の作用点としたヤコビ行列Ｊを用いて演算される（式（３）参照）。

　尚、図６～図８の電流推定部６３で推定される各サーボモータを流れる電流の推定値は、制御装置３の電流指令値生成部６１において、予め定められた位置指令値と位置センサからの関節軸角度位置に基づいて演算された電流指令値でもよいし、その他の公知の方法により、演算されてもよい。また、電流推定部６３は、監視装置４Ｂ～４Ｄに含まれてもよい。

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

　以上の第２乃至第５実施形態の監視装置４Ａ～４Ｄの構成は第１実施形態の監視装置４の構成と実質的に等価であるので、第１実施形態な効果を奏することができる。

　［その他の実施形態］
　尚、上記実施形態では、監視装置４は、外力の値を直接衝突判定に用いる第１衝突判定部５２と、ローパスフィルタ５３を通過させた外力の値を衝突判定に用いる第２衝突判定部５４の双方を備えたが、これに限られるものではなく、第１衝突判定部５２、又は、第２衝突判定部５４のいずれかのみを備えてもよい。

　また、外力の値の変化量を所定の閾値と比較して衝突判定を行ってもよい。これにより、ロボットに働く外力の急激な変動を検知することができる。

　尚、上記実施形態では、ロボット２は、６軸多関節ロボットであったが、１軸以上であれば、これに限られない。また、２つのアームを備えた双腕ロボットでもよい。

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

　本発明は、多関節ロボットの監視に適用可能である。

１　ロボットシステム
２　ロボット
３　制御装置
４，４Ａ～４Ｄ　監視装置
５　電流センサ
６　演算処理器
７　サーボアンプ
４１，４１Ａ，４１Ｂ　電流トルク変換部
４２　差分トルク演算部
４３　外力変換部
４４　停止信号生成部
４５，４５Ａ，４５Ｂ　電流外力変換部
４６　トルク外力変換部
４７　差分電流演算部
５１　加減算器（トルク）
５２　第１衝突判定部
５３　ローパスフィルタ
５４　第２衝突判定部
５５　加減算器（外力）
５６　加減算器（電流）
６１　電流値生成部
６２　駆動トルク推定部
６３　電流推定部
Ｊ１～Ｊ６　関節軸
Ｍ１～Ｍ６　サーボモータ
Ｌ１～Ｌ６　電源ライン

Claims

　１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、
　各サーボモータに流れる電流値を検出する電流センサと、
　前記電流センサで検出された各サーボモータを流れる電流値をトルク値に変換する電流トルク変換部と、
　各サーボモータの駆動に必要な駆動トルクの少なくとも一部を推定する駆動トルク推定部と、
　前記電流トルク変換部で変換されたトルク値と前記駆動トルクの推定値との差分トルクを演算する差分トルク演算部と、
　前記差分トルク演算部で演算された前記差分トルクを前記ロボットの外力に変換する外力変換部と、
　前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、
を備えるロボットシステムの監視装置。
　１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、
　各サーボモータに流れる電流値を検出する電流センサと、
　前記電流センサで検出された各サーボモータを流れる電流値を前記ロボットの外力の検出値に変換する電流外力変換部と、
　各サーボモータの駆動に必要な駆動トルクの少なくとも一部を推定する駆動トルク推定部と、
　前記駆動トルクの推定値を前記ロボットの外力の推定値に変換するトルク外力変換部と、
　前記ロボットの外力の検出値と前記ロボットの外力の推定値との差分を演算することにより前記ロボットの外力に変換する外力変換部と、
　前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、
を備えるロボットシステムの監視装置。
　１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、
　各サーボモータに流れる電流値を検出する電流センサと、
　各サーボモータを流れる電流値を推定する電流推定部と、
　各サーボモータを流れる検出電流値と推定電流値との差分電流を演算する差分電流演算部と、
　前記差分電流演算部で算出された差分電流を前記ロボットの外力に変換する外力変換部と、
　前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、
を備えるロボットシステムの監視装置。
　１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、
　各サーボモータに流れる電流値を検出する電流センサと、
　前記電流センサで検出された各サーボモータを流れる電流値をトルクの検出値に変換する第１電流トルク変換部と、
　各サーボモータを流れる電流値を推定する電流推定部と、
　前記電流推定部で推定された各サーボモータを流れる推定電流値をトルクの推定値に変換する第２電流トルク変換部と、
　前記トルクの検出値と前記トルクの推定値との差分トルクを演算する差分トルク演算部と、
　前記差分トルク演算部で算出された差分トルクを前記ロボットの外力に変換する外力変換部と、
　前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、
を備えるロボットシステムの監視装置。
　１以上の関節軸と、その関節軸を駆動させるサーボモータを備えたロボットと、前記各関節軸を駆動するサーボモータを制御する制御装置と、を備えたロボットシステムの監視装置であって、
　各サーボモータに流れる電流値を検出する電流センサと、
　各サーボモータを流れる電流値を推定する電流推定部と、
　前記電流センサで検出された各サーボモータを流れる電流値を前記ロボットの外力の検出値に変換する第１電流外力変換部と、
　前記電流推定部で推定された各サーボモータを流れる推定電流値を前記ロボットの外力の推定値に変換する第２電流外力変換部と、
　前記ロボットの外力の検出値と前記ロボットの外力の推定値との差分を演算することにより前記ロボットの外力に変換する外力変換部と、
　前記外力変換部で変換された前記外力の値に基づいて前記ロボットの停止信号を生成し、これを前記制御装置に供給する停止信号生成部と、
を備えるロボットシステムの監視装置。
　前記停止信号生成部は、前記外力変換部で変換された前記外力の値が予め設定された第１閾値を超えたときに前記ロボットの停止信号を生成する、請求項１乃至５のいずれか一項に記載のロボットシステムの監視装置。
　前記トルク外力変換部で変換された前記外力の値を入力値とするローパスフィルタを更に備え、
　前記停止信号生成部は、前記ローパスフィルタの出力値が予め設定された第２閾値を超えたときに前記ロボットの停止信号を生成する、請求項１乃至６のいずれか一項に記載のロボットシステムの監視装置。
　トルクから前記ロボットの外力への変換、又は、電流から前記ロボットの外力への変換は、前記ロボットの所定の点を外力の作用点としたヤコビ行列を用いて演算される、請求項１乃至７のいずれか一項に記載のロボットシステムの監視装置。