ロボットシステム
【課題】異物に衝突したか否かにかかわらず、振動的な動作を伴うことなく且つ機構部品に損傷を与えることなくロボットを緊急停止させることができるロボットシステムを提供する。
【解決手段】緊急停止モードにおいて、位置制御部３１は、上位制御部から与えられる位置指令ｐｃにかかわらず出力する速度指令ｖｃをゼロに固定し、通常の位置制御は行わない。速度制御部３２は、モータＭの回転速度ｖ＊をゼロに固定された速度指令ｖｃに一致させるように電流指令演算値ｉｃ’を演算するとともに、その電流指令演算値ｉｃ’に対して所定のフィルタ制御を行って電流指令ｉｃの変化量を所定の制限値以下に制限する。
【解決手段】緊急停止モードにおいて、位置制御部３１は、上位制御部から与えられる位置指令ｐｃにかかわらず出力する速度指令ｖｃをゼロに固定し、通常の位置制御は行わない。速度制御部３２は、モータＭの回転速度ｖ＊をゼロに固定された速度指令ｖｃに一致させるように電流指令演算値ｉｃ’を演算するとともに、その電流指令演算値ｉｃ’に対して所定のフィルタ制御を行って電流指令ｉｃの変化量を所定の制限値以下に制限する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、緊急停止指令が入力されることにより、ロボットの動作を直ちに停止させる制御を行うロボットシステムに関する。
【背景技術】
【０００２】
ロボットシステムでは、非常時にロボットのアームの動作を直ちに停止させる緊急停止制御を行うようになっている。この緊急停止制御では、ロボットの各軸を駆動するモータに逆転トルクを発生させつつ、その後の移動軌跡に沿ってアームの動作を停止させる。すなわち、上記緊急停止制御では、現在位置を指令位置に一致させる位置制御を実行しながらロボットの動作を停止させていた。
【０００３】
このような緊急停止時には、通常動作における停止時に比べて高い加速度（減速度）での停止動作となる。従って、上記緊急停止制御では、高い加速度により停止動作を行うにもかかわらず位置制御を行うため、ロボットのアームの動きが振動的になる可能性がある。すなわち、ロボットのアームが目標の停止位置を一旦通り過ぎてから再び戻るように停止することがある。このような振動的な動作を、ロボットを操作する者（操作者）が予期することは難しい。このため、緊急停止時の操作者の安全性が低下するおそれがあった。
【０００４】
また、上記した振動的な動作においてアームの移動方向が反転する瞬間には、大きな逆転トルクが発生してロボットの関節に多大な負荷がかかることになる。このように関節に大きな負荷がかかると、関節を構成する機構部品に損傷を与えたり、それら部品間の締結が緩んだりする可能性がある。ロボットシステムでは、関節部分が正常であることを前提としてモータをフィードバック制御し、ロボットの動作制御を行っているので、関節部分に上記問題が生じると精度のよい動作制御ができない。
【０００５】
これに対し、特許文献１には、ロボットの衝突後の停止制御方法が開示されている。すなわち、特許文献１記載の技術では、ロボットが異物に衝突したことを検出すると、衝突を検知した軸を駆動するサーボモータへの速度指令をゼロにすることで逆転トルクを発生させて、できるだけ短時間でロボットの動作を停止させている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開平３−２８１１９４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
特許文献１記載の技術によれば、ロボットが異物に衝突したことで緊急停止を行う必要が生じた場合には、ロボットの動作を短時間で停止させることができる。しかし、このような特許文献１記載の技術をロボットが異物に衝突することなく緊急停止を行う場合に適用すると次のような問題が生じる。
【０００８】
すなわち、アームが衝突する瞬間にはモータの回転速度はほとんどゼロとなる。従って、衝突した時点から回転速度をゼロに低下させるために発生させる逆転トルクは比較的小さい。このため、逆転トルクによって関節部に加わる負荷も小さい。これに対し、アームが異物に衝突することなく緊急停止をさせる場合には、停止動作を開始する瞬間のモータの回転速度は、それ以前の回転速度とほぼ同じである。従って、停止動作の開始時点から回転速度をゼロに停止させるために発生させる逆転トルクは大きなものとなる。このため、逆転トルクによって関節部に加わる負荷も大きくなってしまう。
【０００９】
本発明は上記事情に鑑みてなされたものであり、その目的は、異物に衝突したか否かにかかわらず、振動的な動作を伴うことなく且つ機構部品に損傷を与えることなくロボットを緊急停止させることができるロボットシステムを提供することにある。
【課題を解決するための手段】
【００１０】
請求項１記載の手段によれば、ロボットの動作を最短距離で停止させるように指令する緊急停止指令が入力されていないとき、つまり通常時の動作は次のように行われる。すなわち、位置制御部は、位置検出部により検出されるモータの回転位置を、上位制御部から与えられる位置指令に一致させるように速度指令を出力する位置制御を行う。速度制御部は、速度演算部により演算されるモータの回転速度を、位置制御部から与えられる速度指令に一致させるようにモータに所定のトルクを発生させるための電流指令を出力する速度制御を行う。電流制御部は、電流検出部により検出されるモータに流れる電流を、速度制御部から与えられる電流指令に一致させるようにモータの通電指令を駆動部に対して出力する。このような位置制御、速度制御および電流制御が行われることにより、ロボットの各軸を駆動するモータの駆動がフィードバック制御される。
【００１１】
一方、上記緊急停止指令が入力手段を介して入力されたとき、つまり緊急停止時の動作は次のように行われる。すなわち、位置制御部は、上位制御部から与えられる位置指令にかかわらず、出力する速度指令をゼロに固定する。そして、速度制御部は、以下のようなフィルタ制御を行う。このフィルタ制御において、速度制御部は、今回のサンプリング時点において通常時と同様に求めた電流指令である今回電流指令と、前回のサンプリング時点における電流指令である前回電流指令との差を求める。この差は、前回のサンプリング時点から今回のサンプリング時点にかけての電流指令の変化量（増加量または減少量）に相当する。この変化量が予め定められた所定の制限変化量以下である場合には、今回電流指令を電流指令として電流制御部に出力する。また、変化量が制限変化量より大きく、且つ増加量である場合には、前回電流指令に対し、プラスの符号を付した制限変化量を加えたものを電流指令として電流制御部に出力する。また、変化量が制限変化量より大きく、且つ減少量である場合には、前回電流指令に対し、マイナスの符号を付した制限変化量を加えたものを電流指令として電流制御部に出力する。このフィルタ動作は、位置制御部から出力される速度指令がゼロに固定されているため、モータの回転速度がゼロになるまで継続される。
【００１２】
緊急停止時には、上記動作が行われることでモータの駆動を停止する方向に働く電流（逆転トルク）の変化量が所定の制限変化量以下に抑えられる。従って、例えば緊急停止指令が入力された直後などに多大な逆転トルクが発生し、ロボットを構成する機構部品に過大な負担がかかり損傷を与えてしまう事態を未然に防止できる。また、この緊急停止時には、位置制御部から出力される速度指令はゼロに固定されており、通常の位置制御は行われない。このため、速度制御部は、位置制御による制限を受けることなく、回転速度をゼロに収束させるべく速度制御を実行する。従って、振動的な動作を伴うことなく、ロボットを最短距離で緊急停止させることができる。
【００１３】
本手段によれば、通常、ロボットのモータ制御に用いられる位置制御を実行する位置制御部および速度制御を実行する速度制御部に対し、緊急停止指令が入力されたときの上記制御内容を追加するだけで上記各効果が得られる。このため、既存の緊急停止制御からの修正が容易であり、また、制御部の制御内容を簡素化することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施形態を示すロボットシステムの電気構成図
【図２】モータ制御の内容を等価的に示すブロック図
【図３】ロボットシステムの構成を概略的に示す図
【図４】割り込み制御の内容を示すフローチャート
【図５】位置制御部の制御内容を示すフローチャート
【図６】速度制御部の制御内容を示すフローチャート
【図７】電流制御部の制御内容を示すフローチャート
【図８】フィルタ制御の内容を示すフローチャート
【図９】緊急停止時における電流指令および電流指令演算値の変化を表す図
【図１０】緊急停止時における回転速度を表す図
【図１１】緊急停止時における電流指令を表す図
【発明を実施するための形態】
【００１５】
以下、本発明の一実施形態について図面を参照しながら説明する。
図３は、一般的な産業用ロボットのシステム構成を示している。この図３に示すロボットシステム１は、ロボット２と、ロボット２を制御するコントローラ３と、コントローラ３に接続されたティーチングペンダント４（入力手段に相当）とから構成されている。
ロボット２は、例えば６軸の垂直多関節型ロボットとして構成されている。ロボット２は、ベース５と、このベース５に水平方向に回転可能に支持されたショルダ部６と、このショルダ部６に上下方向に回転可能に支持された下アーム７と、この下アーム７に上下方向に回転可能に支持された第１の上アーム８と、この第１の上アーム８に捻り回転可能に支持された第２の上アーム９と、この第２の上アーム９に上下方向に回転可能に支持された手首１０と、この手首１０に捻り回転可能に支持されたフランジ１１とから構成されている。
【００１６】
ベース５、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０およびフランジ１１は、ロボット２のアームとして機能し、アーム先端であるフランジ１１には、図示はしないが、エンドエフェクタ（手先）が取り付けられる。ロボット２に設けられる複数の軸はそれぞれに対応して設けられるモータ（図１に符号Ｍを付して示す）により駆動される。各モータの近傍には、それぞれの回転軸の回転位置を検出するための位置検出器（図示せず）が設けられている
ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４には、各種のキースイッチ１２が設けられており、使用者は、キースイッチ１２により種々の入力操作を行う。このキースイッチ１２には、後述する緊急停止を指示するための緊急停止スイッチが含まれている。使用者は、この緊急停止スイッチを操作することで、ロボット２の動作（アームの動作）を最短距離で停止させることができるようになっている。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、キースイッチ１２の操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。
【００１７】
図１は、ロボットシステム１の電気構成を概略的に示すブロック図である。ロボット２には、各軸をそれぞれ駆動するための複数のモータＭ（図１では１つのみ示す）が設けられている。モータＭはブラシレスＤＣモータである。コントローラ３には、交流電源２１より供給される交流を整流および平滑して出力する直流電源装置２２、モータＭを駆動するインバータ装置２３、電流検出部２４、位置検出部２５およびこれら各装置の制御などを行う制御部２６が設けられている。
【００１８】
直流電源装置２２は、整流回路２７と平滑用のコンデンサ２８とから構成されている。整流回路２７は、ダイオードをブリッジの形態に接続してなる周知構成のものである。例えば３相２００Ｖの交流電源２１の各相出力は、整流回路２７の交流入力端子に接続されている。整流回路２７の直流出力端子は、それぞれ直流電源線２９、３０に接続されている。これら直流電源線２９、３０間にはコンデンサ２８が接続されている。
【００１９】
インバータ装置２３（駆動部に相当））は、直流電源線２９、３０間に６つのスイッチング素子例えばＩＧＢＴ（図１には２つのみ示す）を三相フルブリッジ接続して構成されたインバータ主回路と、その駆動回路とを６組備えている（図１には１組のみ示す）。ＩＧＢＴのコレクタ・エミッタ間には還流ダイオードが接続されている。また、ＩＧＢＴのゲートには、駆動回路からゲート信号が与えられている。駆動回路は、制御部２６から与えられる指令信号（通電指令Ｓｃ）に基づいてパルス幅変調されたゲート信号を出力して各ＩＧＢＴを駆動する。
【００２０】
制御部２６は、ＣＰＵ、ＲＯＭ、ＲＡＭ、Ｉ／Ｏなどを備えたマイクロコンピュータを主体として構成されている。電流検出部２４は、モータＭに流れる電流を検出する電流検出器（図示せず）からの検出信号を制御部２６に入力可能なデータに変換して出力する。位置検出部２５は、モータＭの回転位置を検出する位置検出器（図示せず）からの検出信号を制御部２６に入力可能なデータに変換して出力する。制御部２６は、電流検出部２４から出力されるデータを元にモータＭに流れる電流の値を取得するとともに、位置検出部２５から出力されるデータを元にモータＭの回転位置および回転速度を取得する。詳細は後述するが、制御部２６は、このようにして取得した電流値、回転位置および回転速度を用いてインバータ装置２３によるモータＭの駆動をフィードバック制御する。
【００２１】
図２は、ロボットシステム１におけるモータ制御の内容を等価的に示したブロック図である。この図２に示すように、制御部２６は、位置制御部３１、速度制御部３２および電流制御部３３を備えている。なお、図２では、１つのモータＭの制御に係る構成のみを示しているが、実際には全てのモータＭのそれぞれに対応して同様の構成が設けられている。さて、一般に産業用のロボットは、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作するようになっている。図示しない上位制御部は、その動作プログラムを解釈し、ロボット２に動作プログラムに従った動作を行わせるように各モータＭを制御するための指令値（位置指令ｐｃ）を位置制御部３１に出力する。
【００２２】
位置制御部３１は、上位制御部から与えられる位置指令ｐｃ（モータＭの回転位置の指令値）に対する現在の回転位置ｐ＊の偏差を求める減算器３４と、この減算器３４の出力（偏差）をゼロに近づけるように速度指令ｖｃを出力する位置制御アンプ３５とから構成されている。この位置制御アンプ３５のゲインはＫｐとなっている。
【００２３】
速度制御部３２は、微分器３６、減算器３７、速度制御アンプ３８、フィルタ３９およびリミッタ４０により構成されている。微分器３６（速度演算部に相当）は、現在の回転位置ｐ＊を微分して現在の回転速度ｖ＊に変換する。減算器３７は、速度指令ｖｃに対する現在の回転速度ｖ＊の偏差を求める。速度制御アンプ３８は、この減算器３７の出力（偏差）をゼロに近づけるように電流指令演算値ｉｃ’を出力する。この速度制御アンプ３８のゲインはＫｖとなっている。フィルタ３９は、速度制御アンプ３８の出力信号に対し、所定のフィルタ制御（詳細は後述する）を行う。
【００２４】
リミッタ４０は、フィルタ３９の出力信号が所定の上限値および下限値を超えないように制限を与えるリミッタ制御を行う。すなわち、リミッタ４０は、フィルタ３９の出力信号を上限値および下限値でクリップさせる。なお、これら上限値および下限値は、これらを超える電流がモータＭに流れると故障が生じる可能性があるという値であり、モータＭの回路定数などに基づいて定められている。リミッタ４０は、フィルタ３９の出力信号に上記制限を与えた信号を電流指令ｉｃとして出力する。
【００２５】
電流制御部３３は、電流指令ｉｃに対する現在のモータＭに流れる電流ｉ＊の偏差を求める減算器４１と、この減算器４１の出力（偏差）をゼロに近づけるようにインバータ装置２３に対する指令信号（通電指令Ｓｃ）を出力する電流制御アンプ４２とから構成されている。この電流制御アンプ４２のゲインはＫｉとなっている。このような構成により、制御部２６は、電流フィードバック制御、速度フィードバック制御および位置フィードバック制御を行い、モータＭの駆動をフィードバック制御してロボット２のアームの動作制御を行う。
【００２６】
次に、上記構成の作用について図４〜図１１も参照して説明する。
本実施形態では、通常時（通常モード）と緊急停止時（緊急停止モード）とにおいて、制御部２６によるモータＭの制御動作が変化するようになっている。これら通常モードおよび緊急停止モードは、緊急停止フラグFlgの状態に応じて切り替えられる。この緊急停止フラグFlgは、システムの起動時にリセット（Flg＝０）されている。そして、緊急停止フラグFlgは、図４に示す割り込み制御が実行されることでセット（Flg＝１）される（図４のステップＡ１）。
【００２７】
この割り込み制御は、ロボット２の動作を緊急停止させるための指令（緊急停止指令）が入力されると実行されるもので、例えばティーチングペンダント４の緊急停止スイッチが操作されたときや、制御部２６において何らかの異常（故障）が発生し、以後ロボット２を正常に動作させることが困難であると判断されたときなどに実行される。この割り込み制御が行われて緊急停止フラグFlgがセットされると、モータＭの駆動制御によりアームの動作が停止され（詳細は後述する）、その後、インバータ装置２３への直流電源の供給が停止される。続いて、モータＭの回転軸をロックするブレーキが作動され、その後、上位制御部からの動作プログラムに基づく指令値の出力が停止されるようになっている。
【００２８】
最初に、通常モードにおける制御部２６によるモータＭの制御動作について説明する。通常モードでは、制御部２６は、位置フィードバック制御、速度フィードバック制御および電流フィードバック制御を行ってモータＭの駆動を制御する。すなわち、位置制御部３１は、所定の位置制御周期（例えば５００μｓ）毎に起動し、起動する度に図５のフローチャートに示された内容の制御を実行する。位置制御部３１は、最初のステップＢ１では、モータＭの現在の回転位置ｐ＊を取得する。続くステップＢ２では、緊急停止フラグFlgがセットされているか否かを判断する。
【００２９】
この場合、緊急停止指令が与えられていないため、緊急停止フラグFlgはリセット（Flg＝０）されている（ステップＢ２で「ＮＯ」）。このため、ステップＢ３をスキップしてステップＢ４に進み、位置指令ｐｃ、回転位置ｐ＊および位置ゲインＫｐを用い、下記（１）式のように速度指令ｖｃを演算する。
ｖｃ＝Ｋｐ×（ｐｃ−ｐ＊） …（１）
続くステップＢ５において、上記（１）式に基づいて算出された速度指令ｖｃを速度制御部３２に出力して制御を終了する（エンド）。
【００３０】
速度制御部３２は、所定の速度制御周期（サンプリング周期に相当するものであり、例えば２５０μｓ）毎に起動し、起動する度に図６のフローチャートに示された内容の制御を実行する。速度制御部３２は、最初のステップＣ１では、モータＭの現在の回転速度ｖ＊を取得する。続くステップＣ２では、速度指令ｖｃ、回転速度ｖ＊および速度ゲインＫｖを用い、下記（２）式のように電流指令演算値ｉｃ’を演算する。
ｉｃ’＝Ｋｖ×（ｖｃ−ｖ＊） …（２）
【００３１】
この場合、緊急停止指令が与えられていないため、緊急停止フラグFlgはリセット（Flg＝０）されている（ステップＣ３で「ＮＯ」）。このため、ステップＣ４のフィルタ制御は行わずにステップＣ５に進む。
ステップＣ５では、上記（２）式に基づいて算出された電流指令演算値ｉｃ’に対して所定の制限を与えるリミッタ制御を行う。続くステップＣ６では、ステップＣ５にて制限が与えられた電流指令演算値ｉｃ’を、電流指令ｉｃとして電流制御部３３に出力して制御を終了する（エンド）。
【００３２】
電流制御部３３は、所定の電流制御周期（例えば５０μｓ）毎に起動し、起動する度に図７のフローチャートに示された内容の制御を実行する。電流制御部３３は、最初のステップＤ１では、モータＭに流れる電流ｉ＊を取得する。続くステップＤ２では、電流指令ｉｃ、電流ｉ＊および電流ゲインＫｉを用い、下記（３）式のように通電指令Ｓｃを演算する。
Ｓｃ＝Ｋｉ×（ｉｃ−ｉ＊） …（３）
続くステップＤ３において、上記（３）式に基づいて算出された通電指令Ｓｃをインバータ装置２３に出力して制御を終了する（エンド）。
【００３３】
続いて、緊急停止モードにおける制御部２６によるモータＭの制御動作について説明する。以下では、緊急停止モードでの位置制御部３１および速度制御部３２の動作について通常モードの場合とは異なる部分を主体に説明する。なお、電流制御部３３は、緊急停止モードにおいても通常モードの場合と同様の制御を行うので説明を省略する。
【００３４】
緊急停止モードでは、位置制御部３１は、上位制御部から与えられる位置指令ｐｃにかかわらず速度指令ｖｃをゼロに固定する。すなわち、この場合、緊急停止フラグFlgがセット（Flg＝１）されている（ステップＢ２で「ＹＥＳ」）ため、ステップＢ３に進み、位置ゲインＫｐをゼロにする（Ｋｐ＝０）。これにより、続くステップＢ４で求められる速度指令ｖｃがゼロに固定される。そして、ステップＢ５において、ゼロに固定された速度指令ｖｃを速度制御部３２に出力する。
【００３５】
緊急停止モードでは、速度制御部３２は、電流指令演算値ｉｃ’に対してフィルタ制御を行う。すなわち、この場合、緊急停止フラグFlgがセット（Flg＝１）されている（ステップＣ３で「ＹＥＳ」）ため、ステップＣ４に進みフィルタ制御を実行する。このフィルタ制御の内容は図８に示すものである。まず、最初のステップＥ１では、前回の速度制御起動時点（１速度制御周期前の時点）から今回の速度制御起動時点（現時点）にかけての電流指令演算値ｉｃ’の変化量ａ（増加量または減少量、傾き）を求める。すなわち、ステップＥ１では、今回の速度制御起動時にステップＣ２で演算された電流指令演算値ｉｃ’（今回電流指令に相当するものであり、以下ｉｃ’（ｎ）と表す）と、前回の速度制御起動時にリミッタ４０に出力された電流指令ｉｃ（前回電流指令に相当するものであり、以下ｉｃ（ｎ−１）と表す）とを用い、下記（４）式のように変化量ａを演算する。
ａ＝ｉｃ’（ｎ）−ｉｃ（ｎ−１） …（４）
【００３６】
続くステップＥ２では、変化量ａの絶対値｜ａ｜が、制限値ａlim（制限変化量に相当）を超えているか否かを判断する。変化量ａの絶対値｜ａ｜が制限値ａlim以下である場合（ステップＥ２で「ＮＯ」）、ステップＥ３〜Ｅ５をスキップしてステップＥ６に進む。そして、ステップＥ６において、今回の電流指令演算値ｉｃ’（ｎ）をそのままリミッタ４０に出力し、制御を終了する（リターン）。
【００３７】
一方、変化量ａの絶対値｜ａ｜が制限値ａlimを超えている場合（ステップＥ２で「ＹＥＳ」）、ステップＥ３に進む。ステップＥ３では、変化量ａの極性、つまり変化量ａが増加量（増加方向の傾き、右肩上がりの傾き）であるのか、減少量（減少方向の傾き、右肩下がりの傾き）であるのかを判断する。変化量ａが増加量である場合（ＹＥＳ）にはステップＥ４に進み、減少量である場合（ＮＯ）にはステップＥ５に進む。
【００３８】
ステップＥ４では、前回の電流指令ｉｃ（ｎ−１）に対し、プラスの符号を付した制限値ａlimを加えたもの（ｉｃ（ｎ−１）＋ａlim）を今回の電流指令演算値ｉｃ’（ｎ）とする。ステップＥ５では、前回の電流指令ｉｃ（ｎ−１）に対し、マイナスの符号を付した制限値ａlimを加えたもの（ｉｃ（ｎ−１）−ａlim）を今回の電流指令演算値ｉｃ’（ｎ）とする。続くステップＥ６では、ステップＥ５において求められた電流指令演算値ｉｃ’（ｎ）をリミッタ４０に出力し、制御を終了する（リターン）。
【００３９】
このようなフィルタ制御が行われることより、緊急停止時において速度制御部３２から出力される電流指令ｉｃの前回から今回にかけての変化量（増減量）が制限値ａlim以下に抑えられる。なお、制限値ａlimは、緊急停止時においてロボット２を構成する機構部品に過大な負担がかからないように電流指令ｉｃの変化量を制限できる値に設定すればよい。例えば、事前にロボット２の可動部に加速度計を取り付けた状態で緊急停止させる実験を、電流指令ｉｃの変化量を段階的に変化させて行う。そして、実際に機構部品に異常が生じたときの加速度の計測値から機構部に過大な負担がかかる電流（トルク）の変化量の限界値を求め、その限界値に基づいて制限値ａlimを設定すればよい。
【００４０】
続いて、緊急停止時において速度制御部３２から出力される電流指令ｉｃがどのように変化するかについて図９も参照しながら説明する。なお、図９では、電流指令ｉｃを実線で示し、電流指令演算値ｉｃ’を破線で示し、変化量ａの制限値ａlimを二点鎖線で示し、リミッタ４０の下限値を一点鎖線で示している。また、図９では、中央の実線（電流指令＝０を示す実線）を挟んで、上側がモータＭを正転方向（所定のアーム動作を行わせるための回転方向）に回転させる電流（トルク）であり、下側がモータＭを逆転方向（所定のアーム動作を停止させるための回転方向）に回転させる電流（トルク）である。なお、図９中に示す時刻ｔ０〜ｔ１８は、いずれも速度制御部３２が起動される時点（サンプリング時点に相当）を表している。また、図９では、時刻ｔ０〜ｔ１の間に緊急停止指令が入力され、時刻ｔ１８の時点でアームの動作が停止される場合を想定している。
【００４１】
緊急停止指令が入力された後、最初に速度制御部３２が起動する時刻ｔ１では、モータＭを正転方向に回転する所定の回転速度ｖ＊を一気にゼロにすべく、時刻ｔ１〜ｔ２の間の電流指令演算値ｉｃ’は、右肩下がりに急激に変化する。しかし、この電流指令演算値ｉｃ’の変化量ａ（傾き）は、制限値ａlimよりも大きい。従って、時刻ｔ１〜ｔ２の間の電流指令ｉｃは、その変化量（傾き）が制限値ａlimに一致するように制限される。
【００４２】
これにより、時刻ｔ１〜ｔ２の間において発生する正転方向の電流（トルク）が緩やかに減少され、回転速度ｖ＊も緩やかに低下する。このため、続く時刻ｔ２〜ｔ３の間の電流指令演算値ｉｃ’の変化量ａは、若干小さくなる。しかし、その変化量が未だ制限値ａlimより大きいため、時刻ｔ２〜ｔ３の間の電流指令ｉｃについても、変化量が制限値ａlimに一致するように制限される。時刻ｔ３〜ｔ４の間および時刻ｔ４〜ｔ５の間についても、上記同様に、電流指令ｉｃの変化量が制限値ａlimに一致するように制限される。
【００４３】
続く時刻ｔ５〜ｔ６の間、時刻ｔ６〜ｔ７の間および時刻ｔ７〜ｔ８の間では、演算された電流指令演算値ｉｃ’の変化量ａは制限値ａlim以下になる。従って、これらの間では、演算された電流指令演算値ｉｃ’がそのまま電流指令ｉｃとして出力される。このようにして、電流指令ｉｃは、その変化量（傾き）が制限値ａlim以下となるように、時刻ｔ１の時点の値から緩やかに低下していき、時刻ｔ８の時点で下限値に達する。
【００４４】
電流指令演算値ｉｃ’および電流指令ｉｃは、下限値に達した後（時刻ｔ８〜ｔ１０の間）は、リミッタ制御によりその下限値を超えて低下することなくその値が維持される。その後、時刻ｔ１０〜時刻ｔ１８の間は、電流指令演算値ｉｃ’が右肩上がりに緩やかに変化する。この間の電流指令演算値ｉｃ’の変化量ａについても制限値ａlim以下となっているので、電流指令演算値ｉｃ’がそのまま電流指令ｉｃとして出力される。その後、時刻ｔ１８の時点において、アームの動作が停止されることで回転速度ｖ＊が速度指令ｖｃ（＝０）と等しくなり、電流指令ｉｃがゼロになる。
【００４５】
以上説明したように、本実施形態によれば次のような効果が得られる。
緊急停止モードでは、位置制御部３１は、上位制御部から与えられる位置指令ｐｃにかかわらず速度指令ｖｃをゼロに固定し、通常の位置制御は行わない。このため、本実施形態によれば、緊急停止時にも通常の位置制御を行いながらアームの動作を停止させていた従来技術と比較し、次のような優れた効果を奏する。図１０は、緊急停止時のモータＭの回転速度の変化について本実施形態と従来技術とを比較したものである。図１０では、本実施形態における回転速度を実線で示し、従来技術における回転速度を破線で示している。なお、図１０では、時刻ｔａの時点で緊急停止指令が入力された場合を想定している。
【００４６】
この図１０に示すように、従来技術の場合、時刻ｔａの時点で緊急停止指令が入力された後、所定の制御遅れ時間だけ経過した時刻ｔｂの時点からモータＭの回転速度が低下する。この際、緊急停止であるため、通常動作時と比べて高い減速度（加速度）で速度が低下する。ここで、ロボット２の姿勢や把持している物体の重量によっては、この加速度がモータＭの出力に対して過大となる場合がある。このような場合には、回転速度ｖ＊をゼロに合わせることができずにオーバーシュートしてしまう（図１０の時刻ｔｃ〜ｔｄの間）。さらに、このときも位置制御が行われているため、回転位置ｐ＊を位置指令ｐｃに合わせるべく回転速度ｖ＊を再び上昇させる。このような動作が、回転位置ｐ＊が位置指令ｐｃに等しくなるまで繰り返されることで回転速度ｖ＊が振動的になる（時刻ｔｃ〜ｔｅ）。その結果、緊急停止時におけるアームの動きが振動的になってしまう。
【００４７】
これに対し、本実施形態の場合、時刻ｔａの時点で緊急停止指令が入力された後、所定の制御遅れ時間だけ経過した時刻ｔｂの時点からモータＭの回転速度が低下する点は従来技術と同じである。しかし、位置制御部３１は、速度指令ｖｃをゼロに固定して通常の位置制御を行わないので、速度制御部３２は、位置制御による制限を受けることなく、回転速度ｖ＊をゼロに収束させるべく速度制御を実行する。その結果、回転速度ｖ＊は、振動的になることなく、時刻ｔｂ〜時刻ｔｆの間に緩やかに低下し、アームの動作を最短距離で停止させることができる。
【００４８】
また、緊急停止モードでは、速度制御部３２は、ゼロに固定された速度指令ｖｃと回転速度ｖ＊との差に基づいて演算される電流指令演算値ｉｃ’に対しフィルタ制御を行い、電流指令ｉｃの傾きを所定の制限値ａlim以下に制限する。このため、本実施形態によれば、緊急停止時に速度指令ｖｃをゼロに固定するだけでは得られない次のような優れた効果を奏する。図１１では、本実施形態における電流指令ｉｃを実線で示し、フィルタ制御を行わない場合における電流指令ｉｃを破線で示している。なお、図１１において、本実施形態に係る部分については図９と同じとなっている。
【００４９】
この図９に示すように、フィルタ制御を行わない場合、緊急停止指令が入力された後の最初の制御起動時（時刻ｔ１）に回転速度ｖ＊を一気にゼロにすべく、電流指令ｉｃが反転方向（逆転方向）に急激に変化する。すなわち、緊急停止指令が入力された直後において多大な逆転トルクが発生する（時刻ｔ１〜ｔ２）。この電流（トルク）の大きな変化は、躍度（加加速度）が大きいことになる。その結果、ロボット２を構成する機構部品に過大な負担がかかり損傷を与えてしまう危険性がある。
【００５０】
これに対し、本実施形態の場合、緊急停止指令が入力された後の最初の制御起動時（時刻ｔ１）から電流指令演算値ｉｃ’に対しフィルタ制御を行い、電流指令ｉｃの傾きを所定の制限値ａlim以下に制限する。すなわち、モータＭの駆動を停止する方向に働く電流（逆転トルク）の変化量が所定の制限値以下に抑えられる。このため、本実施形態によれば、緊急停止する際に機構部品に過大な負担をかけることがなくなるため、関節を構成する機構部品に損傷を与えたり、それら部品間の締結が緩んだりすることがない。従って、緊急停止した後にロボット２を再度起動する際、関節部分に問題がないので動作制御の精度が低下することがない。
【００５１】
また、ロボットシステム１では、一般にロボット２のモータＭの制御に用いられる位置制御部３１および速度制御部３２に対し、緊急停止指令が入力されたときの上記制御内容（速度指令ｖｃをゼロに固定するとともに電流指令演算値ｉｃ’に対しフィルタ制御を行う）を追加するだけで上記各効果が得られる。このため、既存の緊急停止制御からの修正が容易であり、また、制御部２６の制御内容を簡素化することができる。
【００５２】
なお、本発明は上記し且つ図面に記載した実施形態に限定されるものではなく、次のような変形または拡張が可能である。
速度制御部３２におけるリミッタ４０は必要に応じて設ければよい。リミッタ４０を省略する場合にはフィルタ３９の出力信号が電流指令ｉｃとなる。
電流指令演算値ｉｃ’の変化量ａが増加量である場合、それまでの期間に回転速度ｖ＊が十分に低下されていることが多く、そのときの変化量ａは制限値ａlim以下である可能性が高い。このような場合には、図８に示すフィルタ制御におけるステップＥ４は省略してもよい。すなわち、変化量ａが増加量である場合（ステップＥ３で「ＹＥＳ」）には、ステップＥ６に進み、今回の電流指令演算値ｉｃ’（ｎ）をそのまま電流指令ｉｃとしてリミッタ４０に出力してもよい。
上記実施形態では、本発明を６軸の垂直多関節型のロボット２に適用した例を説明したが、本発明は、各軸をモータにより駆動する構成のロボット全般に適用可能である。
【符号の説明】
【００５３】
図面中、１はロボットシステム、２はロボット、４はティーチングペンダント（入力手段）、２３はインバータ装置（駆動部）、２４は電流検出部、２５は位置検出部、２６は制御部、３１は位置制御部、３２は速度制御部、３３は電流制御部、３６は微分器（速度演算部）、Ｍはモータを示す。
【技術分野】
【０００１】
本発明は、緊急停止指令が入力されることにより、ロボットの動作を直ちに停止させる制御を行うロボットシステムに関する。
【背景技術】
【０００２】
ロボットシステムでは、非常時にロボットのアームの動作を直ちに停止させる緊急停止制御を行うようになっている。この緊急停止制御では、ロボットの各軸を駆動するモータに逆転トルクを発生させつつ、その後の移動軌跡に沿ってアームの動作を停止させる。すなわち、上記緊急停止制御では、現在位置を指令位置に一致させる位置制御を実行しながらロボットの動作を停止させていた。
【０００３】
このような緊急停止時には、通常動作における停止時に比べて高い加速度（減速度）での停止動作となる。従って、上記緊急停止制御では、高い加速度により停止動作を行うにもかかわらず位置制御を行うため、ロボットのアームの動きが振動的になる可能性がある。すなわち、ロボットのアームが目標の停止位置を一旦通り過ぎてから再び戻るように停止することがある。このような振動的な動作を、ロボットを操作する者（操作者）が予期することは難しい。このため、緊急停止時の操作者の安全性が低下するおそれがあった。
【０００４】
また、上記した振動的な動作においてアームの移動方向が反転する瞬間には、大きな逆転トルクが発生してロボットの関節に多大な負荷がかかることになる。このように関節に大きな負荷がかかると、関節を構成する機構部品に損傷を与えたり、それら部品間の締結が緩んだりする可能性がある。ロボットシステムでは、関節部分が正常であることを前提としてモータをフィードバック制御し、ロボットの動作制御を行っているので、関節部分に上記問題が生じると精度のよい動作制御ができない。
【０００５】
これに対し、特許文献１には、ロボットの衝突後の停止制御方法が開示されている。すなわち、特許文献１記載の技術では、ロボットが異物に衝突したことを検出すると、衝突を検知した軸を駆動するサーボモータへの速度指令をゼロにすることで逆転トルクを発生させて、できるだけ短時間でロボットの動作を停止させている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開平３−２８１１９４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
特許文献１記載の技術によれば、ロボットが異物に衝突したことで緊急停止を行う必要が生じた場合には、ロボットの動作を短時間で停止させることができる。しかし、このような特許文献１記載の技術をロボットが異物に衝突することなく緊急停止を行う場合に適用すると次のような問題が生じる。
【０００８】
すなわち、アームが衝突する瞬間にはモータの回転速度はほとんどゼロとなる。従って、衝突した時点から回転速度をゼロに低下させるために発生させる逆転トルクは比較的小さい。このため、逆転トルクによって関節部に加わる負荷も小さい。これに対し、アームが異物に衝突することなく緊急停止をさせる場合には、停止動作を開始する瞬間のモータの回転速度は、それ以前の回転速度とほぼ同じである。従って、停止動作の開始時点から回転速度をゼロに停止させるために発生させる逆転トルクは大きなものとなる。このため、逆転トルクによって関節部に加わる負荷も大きくなってしまう。
【０００９】
本発明は上記事情に鑑みてなされたものであり、その目的は、異物に衝突したか否かにかかわらず、振動的な動作を伴うことなく且つ機構部品に損傷を与えることなくロボットを緊急停止させることができるロボットシステムを提供することにある。
【課題を解決するための手段】
【００１０】
請求項１記載の手段によれば、ロボットの動作を最短距離で停止させるように指令する緊急停止指令が入力されていないとき、つまり通常時の動作は次のように行われる。すなわち、位置制御部は、位置検出部により検出されるモータの回転位置を、上位制御部から与えられる位置指令に一致させるように速度指令を出力する位置制御を行う。速度制御部は、速度演算部により演算されるモータの回転速度を、位置制御部から与えられる速度指令に一致させるようにモータに所定のトルクを発生させるための電流指令を出力する速度制御を行う。電流制御部は、電流検出部により検出されるモータに流れる電流を、速度制御部から与えられる電流指令に一致させるようにモータの通電指令を駆動部に対して出力する。このような位置制御、速度制御および電流制御が行われることにより、ロボットの各軸を駆動するモータの駆動がフィードバック制御される。
【００１１】
一方、上記緊急停止指令が入力手段を介して入力されたとき、つまり緊急停止時の動作は次のように行われる。すなわち、位置制御部は、上位制御部から与えられる位置指令にかかわらず、出力する速度指令をゼロに固定する。そして、速度制御部は、以下のようなフィルタ制御を行う。このフィルタ制御において、速度制御部は、今回のサンプリング時点において通常時と同様に求めた電流指令である今回電流指令と、前回のサンプリング時点における電流指令である前回電流指令との差を求める。この差は、前回のサンプリング時点から今回のサンプリング時点にかけての電流指令の変化量（増加量または減少量）に相当する。この変化量が予め定められた所定の制限変化量以下である場合には、今回電流指令を電流指令として電流制御部に出力する。また、変化量が制限変化量より大きく、且つ増加量である場合には、前回電流指令に対し、プラスの符号を付した制限変化量を加えたものを電流指令として電流制御部に出力する。また、変化量が制限変化量より大きく、且つ減少量である場合には、前回電流指令に対し、マイナスの符号を付した制限変化量を加えたものを電流指令として電流制御部に出力する。このフィルタ動作は、位置制御部から出力される速度指令がゼロに固定されているため、モータの回転速度がゼロになるまで継続される。
【００１２】
緊急停止時には、上記動作が行われることでモータの駆動を停止する方向に働く電流（逆転トルク）の変化量が所定の制限変化量以下に抑えられる。従って、例えば緊急停止指令が入力された直後などに多大な逆転トルクが発生し、ロボットを構成する機構部品に過大な負担がかかり損傷を与えてしまう事態を未然に防止できる。また、この緊急停止時には、位置制御部から出力される速度指令はゼロに固定されており、通常の位置制御は行われない。このため、速度制御部は、位置制御による制限を受けることなく、回転速度をゼロに収束させるべく速度制御を実行する。従って、振動的な動作を伴うことなく、ロボットを最短距離で緊急停止させることができる。
【００１３】
本手段によれば、通常、ロボットのモータ制御に用いられる位置制御を実行する位置制御部および速度制御を実行する速度制御部に対し、緊急停止指令が入力されたときの上記制御内容を追加するだけで上記各効果が得られる。このため、既存の緊急停止制御からの修正が容易であり、また、制御部の制御内容を簡素化することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施形態を示すロボットシステムの電気構成図
【図２】モータ制御の内容を等価的に示すブロック図
【図３】ロボットシステムの構成を概略的に示す図
【図４】割り込み制御の内容を示すフローチャート
【図５】位置制御部の制御内容を示すフローチャート
【図６】速度制御部の制御内容を示すフローチャート
【図７】電流制御部の制御内容を示すフローチャート
【図８】フィルタ制御の内容を示すフローチャート
【図９】緊急停止時における電流指令および電流指令演算値の変化を表す図
【図１０】緊急停止時における回転速度を表す図
【図１１】緊急停止時における電流指令を表す図
【発明を実施するための形態】
【００１５】
以下、本発明の一実施形態について図面を参照しながら説明する。
図３は、一般的な産業用ロボットのシステム構成を示している。この図３に示すロボットシステム１は、ロボット２と、ロボット２を制御するコントローラ３と、コントローラ３に接続されたティーチングペンダント４（入力手段に相当）とから構成されている。
ロボット２は、例えば６軸の垂直多関節型ロボットとして構成されている。ロボット２は、ベース５と、このベース５に水平方向に回転可能に支持されたショルダ部６と、このショルダ部６に上下方向に回転可能に支持された下アーム７と、この下アーム７に上下方向に回転可能に支持された第１の上アーム８と、この第１の上アーム８に捻り回転可能に支持された第２の上アーム９と、この第２の上アーム９に上下方向に回転可能に支持された手首１０と、この手首１０に捻り回転可能に支持されたフランジ１１とから構成されている。
【００１６】
ベース５、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０およびフランジ１１は、ロボット２のアームとして機能し、アーム先端であるフランジ１１には、図示はしないが、エンドエフェクタ（手先）が取り付けられる。ロボット２に設けられる複数の軸はそれぞれに対応して設けられるモータ（図１に符号Ｍを付して示す）により駆動される。各モータの近傍には、それぞれの回転軸の回転位置を検出するための位置検出器（図示せず）が設けられている
ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４には、各種のキースイッチ１２が設けられており、使用者は、キースイッチ１２により種々の入力操作を行う。このキースイッチ１２には、後述する緊急停止を指示するための緊急停止スイッチが含まれている。使用者は、この緊急停止スイッチを操作することで、ロボット２の動作（アームの動作）を最短距離で停止させることができるようになっている。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、キースイッチ１２の操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。
【００１７】
図１は、ロボットシステム１の電気構成を概略的に示すブロック図である。ロボット２には、各軸をそれぞれ駆動するための複数のモータＭ（図１では１つのみ示す）が設けられている。モータＭはブラシレスＤＣモータである。コントローラ３には、交流電源２１より供給される交流を整流および平滑して出力する直流電源装置２２、モータＭを駆動するインバータ装置２３、電流検出部２４、位置検出部２５およびこれら各装置の制御などを行う制御部２６が設けられている。
【００１８】
直流電源装置２２は、整流回路２７と平滑用のコンデンサ２８とから構成されている。整流回路２７は、ダイオードをブリッジの形態に接続してなる周知構成のものである。例えば３相２００Ｖの交流電源２１の各相出力は、整流回路２７の交流入力端子に接続されている。整流回路２７の直流出力端子は、それぞれ直流電源線２９、３０に接続されている。これら直流電源線２９、３０間にはコンデンサ２８が接続されている。
【００１９】
インバータ装置２３（駆動部に相当））は、直流電源線２９、３０間に６つのスイッチング素子例えばＩＧＢＴ（図１には２つのみ示す）を三相フルブリッジ接続して構成されたインバータ主回路と、その駆動回路とを６組備えている（図１には１組のみ示す）。ＩＧＢＴのコレクタ・エミッタ間には還流ダイオードが接続されている。また、ＩＧＢＴのゲートには、駆動回路からゲート信号が与えられている。駆動回路は、制御部２６から与えられる指令信号（通電指令Ｓｃ）に基づいてパルス幅変調されたゲート信号を出力して各ＩＧＢＴを駆動する。
【００２０】
制御部２６は、ＣＰＵ、ＲＯＭ、ＲＡＭ、Ｉ／Ｏなどを備えたマイクロコンピュータを主体として構成されている。電流検出部２４は、モータＭに流れる電流を検出する電流検出器（図示せず）からの検出信号を制御部２６に入力可能なデータに変換して出力する。位置検出部２５は、モータＭの回転位置を検出する位置検出器（図示せず）からの検出信号を制御部２６に入力可能なデータに変換して出力する。制御部２６は、電流検出部２４から出力されるデータを元にモータＭに流れる電流の値を取得するとともに、位置検出部２５から出力されるデータを元にモータＭの回転位置および回転速度を取得する。詳細は後述するが、制御部２６は、このようにして取得した電流値、回転位置および回転速度を用いてインバータ装置２３によるモータＭの駆動をフィードバック制御する。
【００２１】
図２は、ロボットシステム１におけるモータ制御の内容を等価的に示したブロック図である。この図２に示すように、制御部２６は、位置制御部３１、速度制御部３２および電流制御部３３を備えている。なお、図２では、１つのモータＭの制御に係る構成のみを示しているが、実際には全てのモータＭのそれぞれに対応して同様の構成が設けられている。さて、一般に産業用のロボットは、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作するようになっている。図示しない上位制御部は、その動作プログラムを解釈し、ロボット２に動作プログラムに従った動作を行わせるように各モータＭを制御するための指令値（位置指令ｐｃ）を位置制御部３１に出力する。
【００２２】
位置制御部３１は、上位制御部から与えられる位置指令ｐｃ（モータＭの回転位置の指令値）に対する現在の回転位置ｐ＊の偏差を求める減算器３４と、この減算器３４の出力（偏差）をゼロに近づけるように速度指令ｖｃを出力する位置制御アンプ３５とから構成されている。この位置制御アンプ３５のゲインはＫｐとなっている。
【００２３】
速度制御部３２は、微分器３６、減算器３７、速度制御アンプ３８、フィルタ３９およびリミッタ４０により構成されている。微分器３６（速度演算部に相当）は、現在の回転位置ｐ＊を微分して現在の回転速度ｖ＊に変換する。減算器３７は、速度指令ｖｃに対する現在の回転速度ｖ＊の偏差を求める。速度制御アンプ３８は、この減算器３７の出力（偏差）をゼロに近づけるように電流指令演算値ｉｃ’を出力する。この速度制御アンプ３８のゲインはＫｖとなっている。フィルタ３９は、速度制御アンプ３８の出力信号に対し、所定のフィルタ制御（詳細は後述する）を行う。
【００２４】
リミッタ４０は、フィルタ３９の出力信号が所定の上限値および下限値を超えないように制限を与えるリミッタ制御を行う。すなわち、リミッタ４０は、フィルタ３９の出力信号を上限値および下限値でクリップさせる。なお、これら上限値および下限値は、これらを超える電流がモータＭに流れると故障が生じる可能性があるという値であり、モータＭの回路定数などに基づいて定められている。リミッタ４０は、フィルタ３９の出力信号に上記制限を与えた信号を電流指令ｉｃとして出力する。
【００２５】
電流制御部３３は、電流指令ｉｃに対する現在のモータＭに流れる電流ｉ＊の偏差を求める減算器４１と、この減算器４１の出力（偏差）をゼロに近づけるようにインバータ装置２３に対する指令信号（通電指令Ｓｃ）を出力する電流制御アンプ４２とから構成されている。この電流制御アンプ４２のゲインはＫｉとなっている。このような構成により、制御部２６は、電流フィードバック制御、速度フィードバック制御および位置フィードバック制御を行い、モータＭの駆動をフィードバック制御してロボット２のアームの動作制御を行う。
【００２６】
次に、上記構成の作用について図４〜図１１も参照して説明する。
本実施形態では、通常時（通常モード）と緊急停止時（緊急停止モード）とにおいて、制御部２６によるモータＭの制御動作が変化するようになっている。これら通常モードおよび緊急停止モードは、緊急停止フラグFlgの状態に応じて切り替えられる。この緊急停止フラグFlgは、システムの起動時にリセット（Flg＝０）されている。そして、緊急停止フラグFlgは、図４に示す割り込み制御が実行されることでセット（Flg＝１）される（図４のステップＡ１）。
【００２７】
この割り込み制御は、ロボット２の動作を緊急停止させるための指令（緊急停止指令）が入力されると実行されるもので、例えばティーチングペンダント４の緊急停止スイッチが操作されたときや、制御部２６において何らかの異常（故障）が発生し、以後ロボット２を正常に動作させることが困難であると判断されたときなどに実行される。この割り込み制御が行われて緊急停止フラグFlgがセットされると、モータＭの駆動制御によりアームの動作が停止され（詳細は後述する）、その後、インバータ装置２３への直流電源の供給が停止される。続いて、モータＭの回転軸をロックするブレーキが作動され、その後、上位制御部からの動作プログラムに基づく指令値の出力が停止されるようになっている。
【００２８】
最初に、通常モードにおける制御部２６によるモータＭの制御動作について説明する。通常モードでは、制御部２６は、位置フィードバック制御、速度フィードバック制御および電流フィードバック制御を行ってモータＭの駆動を制御する。すなわち、位置制御部３１は、所定の位置制御周期（例えば５００μｓ）毎に起動し、起動する度に図５のフローチャートに示された内容の制御を実行する。位置制御部３１は、最初のステップＢ１では、モータＭの現在の回転位置ｐ＊を取得する。続くステップＢ２では、緊急停止フラグFlgがセットされているか否かを判断する。
【００２９】
この場合、緊急停止指令が与えられていないため、緊急停止フラグFlgはリセット（Flg＝０）されている（ステップＢ２で「ＮＯ」）。このため、ステップＢ３をスキップしてステップＢ４に進み、位置指令ｐｃ、回転位置ｐ＊および位置ゲインＫｐを用い、下記（１）式のように速度指令ｖｃを演算する。
ｖｃ＝Ｋｐ×（ｐｃ−ｐ＊） …（１）
続くステップＢ５において、上記（１）式に基づいて算出された速度指令ｖｃを速度制御部３２に出力して制御を終了する（エンド）。
【００３０】
速度制御部３２は、所定の速度制御周期（サンプリング周期に相当するものであり、例えば２５０μｓ）毎に起動し、起動する度に図６のフローチャートに示された内容の制御を実行する。速度制御部３２は、最初のステップＣ１では、モータＭの現在の回転速度ｖ＊を取得する。続くステップＣ２では、速度指令ｖｃ、回転速度ｖ＊および速度ゲインＫｖを用い、下記（２）式のように電流指令演算値ｉｃ’を演算する。
ｉｃ’＝Ｋｖ×（ｖｃ−ｖ＊） …（２）
【００３１】
この場合、緊急停止指令が与えられていないため、緊急停止フラグFlgはリセット（Flg＝０）されている（ステップＣ３で「ＮＯ」）。このため、ステップＣ４のフィルタ制御は行わずにステップＣ５に進む。
ステップＣ５では、上記（２）式に基づいて算出された電流指令演算値ｉｃ’に対して所定の制限を与えるリミッタ制御を行う。続くステップＣ６では、ステップＣ５にて制限が与えられた電流指令演算値ｉｃ’を、電流指令ｉｃとして電流制御部３３に出力して制御を終了する（エンド）。
【００３２】
電流制御部３３は、所定の電流制御周期（例えば５０μｓ）毎に起動し、起動する度に図７のフローチャートに示された内容の制御を実行する。電流制御部３３は、最初のステップＤ１では、モータＭに流れる電流ｉ＊を取得する。続くステップＤ２では、電流指令ｉｃ、電流ｉ＊および電流ゲインＫｉを用い、下記（３）式のように通電指令Ｓｃを演算する。
Ｓｃ＝Ｋｉ×（ｉｃ−ｉ＊） …（３）
続くステップＤ３において、上記（３）式に基づいて算出された通電指令Ｓｃをインバータ装置２３に出力して制御を終了する（エンド）。
【００３３】
続いて、緊急停止モードにおける制御部２６によるモータＭの制御動作について説明する。以下では、緊急停止モードでの位置制御部３１および速度制御部３２の動作について通常モードの場合とは異なる部分を主体に説明する。なお、電流制御部３３は、緊急停止モードにおいても通常モードの場合と同様の制御を行うので説明を省略する。
【００３４】
緊急停止モードでは、位置制御部３１は、上位制御部から与えられる位置指令ｐｃにかかわらず速度指令ｖｃをゼロに固定する。すなわち、この場合、緊急停止フラグFlgがセット（Flg＝１）されている（ステップＢ２で「ＹＥＳ」）ため、ステップＢ３に進み、位置ゲインＫｐをゼロにする（Ｋｐ＝０）。これにより、続くステップＢ４で求められる速度指令ｖｃがゼロに固定される。そして、ステップＢ５において、ゼロに固定された速度指令ｖｃを速度制御部３２に出力する。
【００３５】
緊急停止モードでは、速度制御部３２は、電流指令演算値ｉｃ’に対してフィルタ制御を行う。すなわち、この場合、緊急停止フラグFlgがセット（Flg＝１）されている（ステップＣ３で「ＹＥＳ」）ため、ステップＣ４に進みフィルタ制御を実行する。このフィルタ制御の内容は図８に示すものである。まず、最初のステップＥ１では、前回の速度制御起動時点（１速度制御周期前の時点）から今回の速度制御起動時点（現時点）にかけての電流指令演算値ｉｃ’の変化量ａ（増加量または減少量、傾き）を求める。すなわち、ステップＥ１では、今回の速度制御起動時にステップＣ２で演算された電流指令演算値ｉｃ’（今回電流指令に相当するものであり、以下ｉｃ’（ｎ）と表す）と、前回の速度制御起動時にリミッタ４０に出力された電流指令ｉｃ（前回電流指令に相当するものであり、以下ｉｃ（ｎ−１）と表す）とを用い、下記（４）式のように変化量ａを演算する。
ａ＝ｉｃ’（ｎ）−ｉｃ（ｎ−１） …（４）
【００３６】
続くステップＥ２では、変化量ａの絶対値｜ａ｜が、制限値ａlim（制限変化量に相当）を超えているか否かを判断する。変化量ａの絶対値｜ａ｜が制限値ａlim以下である場合（ステップＥ２で「ＮＯ」）、ステップＥ３〜Ｅ５をスキップしてステップＥ６に進む。そして、ステップＥ６において、今回の電流指令演算値ｉｃ’（ｎ）をそのままリミッタ４０に出力し、制御を終了する（リターン）。
【００３７】
一方、変化量ａの絶対値｜ａ｜が制限値ａlimを超えている場合（ステップＥ２で「ＹＥＳ」）、ステップＥ３に進む。ステップＥ３では、変化量ａの極性、つまり変化量ａが増加量（増加方向の傾き、右肩上がりの傾き）であるのか、減少量（減少方向の傾き、右肩下がりの傾き）であるのかを判断する。変化量ａが増加量である場合（ＹＥＳ）にはステップＥ４に進み、減少量である場合（ＮＯ）にはステップＥ５に進む。
【００３８】
ステップＥ４では、前回の電流指令ｉｃ（ｎ−１）に対し、プラスの符号を付した制限値ａlimを加えたもの（ｉｃ（ｎ−１）＋ａlim）を今回の電流指令演算値ｉｃ’（ｎ）とする。ステップＥ５では、前回の電流指令ｉｃ（ｎ−１）に対し、マイナスの符号を付した制限値ａlimを加えたもの（ｉｃ（ｎ−１）−ａlim）を今回の電流指令演算値ｉｃ’（ｎ）とする。続くステップＥ６では、ステップＥ５において求められた電流指令演算値ｉｃ’（ｎ）をリミッタ４０に出力し、制御を終了する（リターン）。
【００３９】
このようなフィルタ制御が行われることより、緊急停止時において速度制御部３２から出力される電流指令ｉｃの前回から今回にかけての変化量（増減量）が制限値ａlim以下に抑えられる。なお、制限値ａlimは、緊急停止時においてロボット２を構成する機構部品に過大な負担がかからないように電流指令ｉｃの変化量を制限できる値に設定すればよい。例えば、事前にロボット２の可動部に加速度計を取り付けた状態で緊急停止させる実験を、電流指令ｉｃの変化量を段階的に変化させて行う。そして、実際に機構部品に異常が生じたときの加速度の計測値から機構部に過大な負担がかかる電流（トルク）の変化量の限界値を求め、その限界値に基づいて制限値ａlimを設定すればよい。
【００４０】
続いて、緊急停止時において速度制御部３２から出力される電流指令ｉｃがどのように変化するかについて図９も参照しながら説明する。なお、図９では、電流指令ｉｃを実線で示し、電流指令演算値ｉｃ’を破線で示し、変化量ａの制限値ａlimを二点鎖線で示し、リミッタ４０の下限値を一点鎖線で示している。また、図９では、中央の実線（電流指令＝０を示す実線）を挟んで、上側がモータＭを正転方向（所定のアーム動作を行わせるための回転方向）に回転させる電流（トルク）であり、下側がモータＭを逆転方向（所定のアーム動作を停止させるための回転方向）に回転させる電流（トルク）である。なお、図９中に示す時刻ｔ０〜ｔ１８は、いずれも速度制御部３２が起動される時点（サンプリング時点に相当）を表している。また、図９では、時刻ｔ０〜ｔ１の間に緊急停止指令が入力され、時刻ｔ１８の時点でアームの動作が停止される場合を想定している。
【００４１】
緊急停止指令が入力された後、最初に速度制御部３２が起動する時刻ｔ１では、モータＭを正転方向に回転する所定の回転速度ｖ＊を一気にゼロにすべく、時刻ｔ１〜ｔ２の間の電流指令演算値ｉｃ’は、右肩下がりに急激に変化する。しかし、この電流指令演算値ｉｃ’の変化量ａ（傾き）は、制限値ａlimよりも大きい。従って、時刻ｔ１〜ｔ２の間の電流指令ｉｃは、その変化量（傾き）が制限値ａlimに一致するように制限される。
【００４２】
これにより、時刻ｔ１〜ｔ２の間において発生する正転方向の電流（トルク）が緩やかに減少され、回転速度ｖ＊も緩やかに低下する。このため、続く時刻ｔ２〜ｔ３の間の電流指令演算値ｉｃ’の変化量ａは、若干小さくなる。しかし、その変化量が未だ制限値ａlimより大きいため、時刻ｔ２〜ｔ３の間の電流指令ｉｃについても、変化量が制限値ａlimに一致するように制限される。時刻ｔ３〜ｔ４の間および時刻ｔ４〜ｔ５の間についても、上記同様に、電流指令ｉｃの変化量が制限値ａlimに一致するように制限される。
【００４３】
続く時刻ｔ５〜ｔ６の間、時刻ｔ６〜ｔ７の間および時刻ｔ７〜ｔ８の間では、演算された電流指令演算値ｉｃ’の変化量ａは制限値ａlim以下になる。従って、これらの間では、演算された電流指令演算値ｉｃ’がそのまま電流指令ｉｃとして出力される。このようにして、電流指令ｉｃは、その変化量（傾き）が制限値ａlim以下となるように、時刻ｔ１の時点の値から緩やかに低下していき、時刻ｔ８の時点で下限値に達する。
【００４４】
電流指令演算値ｉｃ’および電流指令ｉｃは、下限値に達した後（時刻ｔ８〜ｔ１０の間）は、リミッタ制御によりその下限値を超えて低下することなくその値が維持される。その後、時刻ｔ１０〜時刻ｔ１８の間は、電流指令演算値ｉｃ’が右肩上がりに緩やかに変化する。この間の電流指令演算値ｉｃ’の変化量ａについても制限値ａlim以下となっているので、電流指令演算値ｉｃ’がそのまま電流指令ｉｃとして出力される。その後、時刻ｔ１８の時点において、アームの動作が停止されることで回転速度ｖ＊が速度指令ｖｃ（＝０）と等しくなり、電流指令ｉｃがゼロになる。
【００４５】
以上説明したように、本実施形態によれば次のような効果が得られる。
緊急停止モードでは、位置制御部３１は、上位制御部から与えられる位置指令ｐｃにかかわらず速度指令ｖｃをゼロに固定し、通常の位置制御は行わない。このため、本実施形態によれば、緊急停止時にも通常の位置制御を行いながらアームの動作を停止させていた従来技術と比較し、次のような優れた効果を奏する。図１０は、緊急停止時のモータＭの回転速度の変化について本実施形態と従来技術とを比較したものである。図１０では、本実施形態における回転速度を実線で示し、従来技術における回転速度を破線で示している。なお、図１０では、時刻ｔａの時点で緊急停止指令が入力された場合を想定している。
【００４６】
この図１０に示すように、従来技術の場合、時刻ｔａの時点で緊急停止指令が入力された後、所定の制御遅れ時間だけ経過した時刻ｔｂの時点からモータＭの回転速度が低下する。この際、緊急停止であるため、通常動作時と比べて高い減速度（加速度）で速度が低下する。ここで、ロボット２の姿勢や把持している物体の重量によっては、この加速度がモータＭの出力に対して過大となる場合がある。このような場合には、回転速度ｖ＊をゼロに合わせることができずにオーバーシュートしてしまう（図１０の時刻ｔｃ〜ｔｄの間）。さらに、このときも位置制御が行われているため、回転位置ｐ＊を位置指令ｐｃに合わせるべく回転速度ｖ＊を再び上昇させる。このような動作が、回転位置ｐ＊が位置指令ｐｃに等しくなるまで繰り返されることで回転速度ｖ＊が振動的になる（時刻ｔｃ〜ｔｅ）。その結果、緊急停止時におけるアームの動きが振動的になってしまう。
【００４７】
これに対し、本実施形態の場合、時刻ｔａの時点で緊急停止指令が入力された後、所定の制御遅れ時間だけ経過した時刻ｔｂの時点からモータＭの回転速度が低下する点は従来技術と同じである。しかし、位置制御部３１は、速度指令ｖｃをゼロに固定して通常の位置制御を行わないので、速度制御部３２は、位置制御による制限を受けることなく、回転速度ｖ＊をゼロに収束させるべく速度制御を実行する。その結果、回転速度ｖ＊は、振動的になることなく、時刻ｔｂ〜時刻ｔｆの間に緩やかに低下し、アームの動作を最短距離で停止させることができる。
【００４８】
また、緊急停止モードでは、速度制御部３２は、ゼロに固定された速度指令ｖｃと回転速度ｖ＊との差に基づいて演算される電流指令演算値ｉｃ’に対しフィルタ制御を行い、電流指令ｉｃの傾きを所定の制限値ａlim以下に制限する。このため、本実施形態によれば、緊急停止時に速度指令ｖｃをゼロに固定するだけでは得られない次のような優れた効果を奏する。図１１では、本実施形態における電流指令ｉｃを実線で示し、フィルタ制御を行わない場合における電流指令ｉｃを破線で示している。なお、図１１において、本実施形態に係る部分については図９と同じとなっている。
【００４９】
この図９に示すように、フィルタ制御を行わない場合、緊急停止指令が入力された後の最初の制御起動時（時刻ｔ１）に回転速度ｖ＊を一気にゼロにすべく、電流指令ｉｃが反転方向（逆転方向）に急激に変化する。すなわち、緊急停止指令が入力された直後において多大な逆転トルクが発生する（時刻ｔ１〜ｔ２）。この電流（トルク）の大きな変化は、躍度（加加速度）が大きいことになる。その結果、ロボット２を構成する機構部品に過大な負担がかかり損傷を与えてしまう危険性がある。
【００５０】
これに対し、本実施形態の場合、緊急停止指令が入力された後の最初の制御起動時（時刻ｔ１）から電流指令演算値ｉｃ’に対しフィルタ制御を行い、電流指令ｉｃの傾きを所定の制限値ａlim以下に制限する。すなわち、モータＭの駆動を停止する方向に働く電流（逆転トルク）の変化量が所定の制限値以下に抑えられる。このため、本実施形態によれば、緊急停止する際に機構部品に過大な負担をかけることがなくなるため、関節を構成する機構部品に損傷を与えたり、それら部品間の締結が緩んだりすることがない。従って、緊急停止した後にロボット２を再度起動する際、関節部分に問題がないので動作制御の精度が低下することがない。
【００５１】
また、ロボットシステム１では、一般にロボット２のモータＭの制御に用いられる位置制御部３１および速度制御部３２に対し、緊急停止指令が入力されたときの上記制御内容（速度指令ｖｃをゼロに固定するとともに電流指令演算値ｉｃ’に対しフィルタ制御を行う）を追加するだけで上記各効果が得られる。このため、既存の緊急停止制御からの修正が容易であり、また、制御部２６の制御内容を簡素化することができる。
【００５２】
なお、本発明は上記し且つ図面に記載した実施形態に限定されるものではなく、次のような変形または拡張が可能である。
速度制御部３２におけるリミッタ４０は必要に応じて設ければよい。リミッタ４０を省略する場合にはフィルタ３９の出力信号が電流指令ｉｃとなる。
電流指令演算値ｉｃ’の変化量ａが増加量である場合、それまでの期間に回転速度ｖ＊が十分に低下されていることが多く、そのときの変化量ａは制限値ａlim以下である可能性が高い。このような場合には、図８に示すフィルタ制御におけるステップＥ４は省略してもよい。すなわち、変化量ａが増加量である場合（ステップＥ３で「ＹＥＳ」）には、ステップＥ６に進み、今回の電流指令演算値ｉｃ’（ｎ）をそのまま電流指令ｉｃとしてリミッタ４０に出力してもよい。
上記実施形態では、本発明を６軸の垂直多関節型のロボット２に適用した例を説明したが、本発明は、各軸をモータにより駆動する構成のロボット全般に適用可能である。
【符号の説明】
【００５３】
図面中、１はロボットシステム、２はロボット、４はティーチングペンダント（入力手段）、２３はインバータ装置（駆動部）、２４は電流検出部、２５は位置検出部、２６は制御部、３１は位置制御部、３２は速度制御部、３３は電流制御部、３６は微分器（速度演算部）、Ｍはモータを示す。
【特許請求の範囲】
【請求項１】
ロボットの各軸を駆動するためのモータと、
前記モータに通電を行い駆動する駆動部と、
前記駆動部による前記モータの駆動を制御する制御部と、
前記ロボットの動作を最短距離で停止させるように指令する緊急停止指令を入力する入力手段と、
前記モータの回転位置を検出する位置検出部と、
前記位置検出部により検出された前記回転位置を用いて前記モータの回転速度を演算する速度演算部と、
前記モータに流れる電流を検出する電流検出部とを備え
前記制御部は、
前記位置検出部により検出される前記回転位置を上位制御部から与えられる位置指令に一致させるように速度指令を出力する位置制御を行う位置制御部と、
前記速度演算部により演算される前記回転速度を前記位置制御部から与えられる速度指令に一致させるように前記モータに所定のトルクを発生させるための電流指令を出力する速度制御を行う速度制御部と、
前記電流検出部により検出される前記電流を前記速度制御部から与えられる電流指令に一致させるように前記モータの通電指令を前記駆動部に対して出力する電流制御を行う電流制御部とを備え、
前記位置制御部は、前記緊急停止指令が入力されると、前記上位制御部から与えられる回転位置指令にかかわらず前記速度指令をゼロに固定し、
前記速度制御部は、
所定のサンプリング周期毎に前記速度制御を実行し、
前記緊急停止指令が入力されると、今回のサンプリング時点における電流指令である今回電流指令と前回のサンプリング時点における電流指令である前回電流指令との差から前回のサンプリング時点から今回のサンプリング時点までの電流指令の変化量を求め、前記変化量が予め定められた所定の制限変化量以下である場合には前記今回電流指令を電流指令として出力し、前記変化量が前記制限変化量より大きく且つ増加量である場合には、前記前回電流指令に対しプラスの符号を付した前記制限変化量を加えたものを電流指令として出力し、前記変化量が前記制限変化量より大きく且つ減少量である場合には、前記前回電流指令に対しマイナスの符号を付した前記制限変化量を加えたものを電流指令として出力するフィルタ制御を行うことを特徴とするロボットシステム。
【請求項１】
ロボットの各軸を駆動するためのモータと、
前記モータに通電を行い駆動する駆動部と、
前記駆動部による前記モータの駆動を制御する制御部と、
前記ロボットの動作を最短距離で停止させるように指令する緊急停止指令を入力する入力手段と、
前記モータの回転位置を検出する位置検出部と、
前記位置検出部により検出された前記回転位置を用いて前記モータの回転速度を演算する速度演算部と、
前記モータに流れる電流を検出する電流検出部とを備え
前記制御部は、
前記位置検出部により検出される前記回転位置を上位制御部から与えられる位置指令に一致させるように速度指令を出力する位置制御を行う位置制御部と、
前記速度演算部により演算される前記回転速度を前記位置制御部から与えられる速度指令に一致させるように前記モータに所定のトルクを発生させるための電流指令を出力する速度制御を行う速度制御部と、
前記電流検出部により検出される前記電流を前記速度制御部から与えられる電流指令に一致させるように前記モータの通電指令を前記駆動部に対して出力する電流制御を行う電流制御部とを備え、
前記位置制御部は、前記緊急停止指令が入力されると、前記上位制御部から与えられる回転位置指令にかかわらず前記速度指令をゼロに固定し、
前記速度制御部は、
所定のサンプリング周期毎に前記速度制御を実行し、
前記緊急停止指令が入力されると、今回のサンプリング時点における電流指令である今回電流指令と前回のサンプリング時点における電流指令である前回電流指令との差から前回のサンプリング時点から今回のサンプリング時点までの電流指令の変化量を求め、前記変化量が予め定められた所定の制限変化量以下である場合には前記今回電流指令を電流指令として出力し、前記変化量が前記制限変化量より大きく且つ増加量である場合には、前記前回電流指令に対しプラスの符号を付した前記制限変化量を加えたものを電流指令として出力し、前記変化量が前記制限変化量より大きく且つ減少量である場合には、前記前回電流指令に対しマイナスの符号を付した前記制限変化量を加えたものを電流指令として出力するフィルタ制御を行うことを特徴とするロボットシステム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１１−６２７９４（Ｐ２０１１−６２７９４Ａ）
【公開日】平成２３年３月３１日（２０１１．３．３１）
【国際特許分類】
【出願番号】特願２００９−２１７２５５（Ｐ２００９−２１７２５５）
【出願日】平成２１年９月１８日（２００９．９．１８）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
【公開日】平成２３年３月３１日（２０１１．３．３１）
【国際特許分類】
【出願日】平成２１年９月１８日（２００９．９．１８）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
[ Back to top ]