(57)【要約】本発明のモータ制御装置は、非常停止指示部により非常停止されたことを移動指令生成部が検知して、移動指令生成部からサーボ制御部への移動指令の送出が停止したとき、偏差補正部で補正値を算出し、サーボ制御部を構成する位置制御部内の偏差カウンタ部の値をこの補正値に置き換えて制御を継続する。偏差補正部が出力する補正値は、位置制御部が出力する速度指令値で非常停止直前の値と位置ゲインの値とから算出する。偏差カウンタ部は、移動指令生成部の出力と、モータに関する情報を出力する処理部の出力との偏差を求める。
2013005388.tif 000002
【特許請求の範囲】
【請求項１】
機械の可動部の相対移動を行いブレーキにより停止されるモータの制御を行うモータ制御装置であって、
前記モータの移動指令を出力する移動指令生成部と、
前記移動指令生成部の出力に基づいて前記モータを制御するための指令を出力するサーボ制御部とを備え、
前記サーボ制御部は、
前記移動指令生成部の出力に基づいて速度指令を出力する位置制御部と、
前記位置制御部の出力に基づいて電流指令を出力する速度制御部と、
前記速度制御部の出力に基づいて電圧指令を出力する電流制御部と、
前記モータの回転位置を検出する位置検出器の出力に基づいて前記モータの回転角の変化量を出力する処理部と、を備え、
前記位置制御部は、
前記移動指令生成部の出力と前記処理部の出力との偏差を求めて出力する偏差カウンタ部と、
前記移動指令生成部の出力を入力して所定の係数を乗じて出力する第１の係数部と、
前記偏差カウンタ部の出力を入力して所定の係数を乗じて出力する第２の係数部と、
前記第１の係数部の出力と前記第２の係数部の出力を加算して前記速度制御部に出力する加算部と、
前記モータ制御装置および前記モータ制御装置の外部のうちの少なくともいずれかに設けられた非常停止指示部により前記モータの非常停止が指示されると、非常停止直前の速度指令である非常停止直前の位置制御周期で前記位置制御部が出力した速度指令値と前記第２の係数部の所定の係数とに基づいて偏差補正値を求める偏差補正部と、を備え、
前記非常停止指示部により非常停止が指示されると、前記移動指令生成部による移動指令の出力を停止し、前記偏差カウンタ部の偏差を前記偏差補正部で求めた偏差補正値に置き換えて前記位置制御部による制御を行う
モータ制御装置。
【請求項２】
前記偏差カウンタ部の偏差の偏差補正部で求めた偏差補正値への置き換えは、非常停止が行われた直後の最初の位置制御周期で１回だけ行い、それ以後は、前記偏差カウンタ部の値と前記処理部の出力との偏差に基づいて制御を行う請求項１記載のモータ制御装置。
【請求項３】
機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、非常停止が指示される直前の前記位置制御部が出力する速度指令値とに基づいて、係数の補正値を求める係数補正部を備え、
前記非常停止指示部により非常停止が指示されると、前記第２の係数部の所定の係数を前記係数補正部で求めた係数補正値に置き換えて前記位置制御部による制御を行う請求項１または２のいずれか１項に記載のモータ制御装置。
【請求項４】
前記第２の係数部の所定の係数の係数補正部で求めた係数補正値への置き換えは、非常停止が行われた直後の最初の位置制御周期で１回だけ行い、それ以後は、置き換えた前記係数補正値を用いて制御を行う請求項３記載のモータ制御装置。
【請求項５】
前記非常停止指示部により非常停止が指示されて前記偏差カウンタ部の偏差を前記偏差補正部で求めた偏差補正値に置き換えた後に、前記偏差カウンタ部の値を位置制御周期毎に補正する偏差更新部を備え、
前記偏差更新部は、非常停止直前の速度指令値と、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、位置制御周期と、前記偏差補正部で求めた偏差補正値と、前記処理部の出力である実位置変化量を差し引く前の前記偏差カウンタ部の値である前回偏差に基づいて、前記位置制御周期毎に前記実位置変化量を差し引いた後の前記偏差カウンタ部の値としての今回偏差を求め、前記位置制御周期毎に前記偏差カウンタ部の偏差を求めた前記今回偏差に置き換えて制御を行う請求項１または２のいずれか１項に記載のモータ制御装置。
【請求項６】
位置制御周期毎に前記処理部の出力を補正して前記偏差カウンタ部に出力する第１の位置補正部を備え、
前記第１の位置補正部は、前記処理部で求めた実位置変化量と、非常停止直前の速度指令値と、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、位置制御周期と、前記偏差補正部で求めた偏差補正値に基づいて、前記偏差カウンタ部に出力する位置フィードバック量を求める請求項１または２のいずれか１項に記載のモータ制御装置。
【請求項７】
位置制御周期毎に前記処理部の出力を補正して前記偏差カウンタ部に出力する第２の位置補正部を備え、
前記第２の位置補正部は、前記処理部で求めた実位置変化量を位置制御周期毎に積算した積算量と、非常停止直前の速度指令値と、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、位置制御周期と、前記偏差補正部で求めた偏差補正値に基づいて、前記偏差カウンタ部に出力する位置フィードバック量を求める請求項１または２のいずれか１項に記載のモータ制御装置。
【請求項８】
前記非常停止指示部により非常停止が指示されると、前記実位置変化量の積算量をゼロにした後に前記実位置変化量の積算を行い、前記実位置変化量の前記積算量をゼロにする処理は、前記非常停止がされたことを検知して以後一度だけ行われる請求項７記載のモータ制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置等のモータ制御装置を用いたシステムにおける非常停止時のモータの停止制御に関する。
【背景技術】
【０００２】
ロボット制御装置等のモータ制御装置を使ったロボットシステムの一例について、図１３を用いて説明する。なお、ロボットシステムは、モータ等により駆動されるマニピュレータと、マニピュレータを制御するためのロボット制御装置等から構成される。【０００３】
図１３は、従来のロボットシステムの概略構成を示す図である。図１３に示すロボットシステムを構成するロボット制御装置１００は、移動指令生成部１０１と、サーボ制御部６０と、アンプ１０６と、を備えている。ここで、移動指令生成部１０１は、ユーザが作成する動作プログラムに基づいて軌道計画をして移動指令を生成する。サーボ制御部６０は、移動指令生成部１０１が出力した移動指令を受けてモータ１０７を駆動制御する。アンプ１０６は、サーボ制御部６０の出力に基づいてモータ１０７を制御する。【０００４】
図１３に示すように、サーボ制御部６０は、位置制御部６１と、速度制御部１０４と、電流制御部１０５と、処理部１１４と、を備えている。これら各制御部は、一定周期ごとに制御演算を行うデジタル制御系で構成される。【０００５】
位置制御部６１は、移動指令生成部１０１からの移動指令を受けて位置制御を行い、速度指令を生成する。速度制御部１０４は、位置制御部６１からの速度指令を受けて速度制御を行い、電流指令を生成する。電流制御部１０５は、速度制御部１０４からの電流指令を受けて電流制御を行い、電圧指令を生成する。アンプ１０６は、電流制御部１０５からの電圧指令を受けてモータ１０７に与えるモータ電流を生成する。【０００６】
移動指令生成部１０１からサーボ制御部６０へ渡される移動指令は、所定の時間毎のモータ１０７の回転角の変化量の形をとる。位置制御部６１は、偏差カウンタ部１１５を構成要素としている。位置制御部６１では、位置制御周期毎に、移動指令を偏差カウンタ部１１５に加算し、一方で、処理部１１４の出力である実位置の位置制御周期毎のモータ１０７の実際の回転角の変化量を偏差カウンタ部１１５から減算する。なお、位置制御周期毎のモータ１０７の回転角の変化量は、モータ１０７に付随する位置検出器１０８が検出したモータ１０７の回転角に基づいて処理部１１４において算出されるものである。このように処理した偏差カウンタ部１１５の偏差カウンタ値に対し、位置ゲイン１１６の係数を乗じて速度指令とする。さらに、これに加えて、移動指令生成部１０１からの移動指令にフィードフォワード係数１１７の係数を乗じた値を速度指令の一部として加算するフィードフォワード制御を備えている。すなわち、位置ゲイン１１６の出力とフィードフォワード係数１１７の出力とを併せたものが、位置制御部６１が出力する速度指令となる。【０００７】
また、モータ１０７には、励磁制御を行わないときに外力で不用意に動いてしまうことを防ぐためのブレーキ１０９が備えられている。このブレーキ１０９は、通常は、保持されてブレーキがかかった状態であり、電流を通じることでブレーキが解除される。【０００８】
また、ロボット制御装置１００およびロボット制御装置１００の外部のうちの少なくともいずれかには、モータ１０７の回転を速やかにかつ直接的に停止するための手段として、非常停止指示部１１２が設けられている。この非常停止指示部１１２が働いたとき、速やかにブレーキ１０９は保持される。すなわち、モータ１０７に対してブレーキ１０９がかかった状態となる。また、非常停止指示部１１２が働いたことにより、移動指令生成部１０１からの移動指令の送出は停止される。【０００９】
具体的には、教示操作時に用いる教示装置に設けられたデッドマンスイッチ１１０や非常停止スイッチ１１１が、ブレーキ１０９の通電回路に連動している。これにより、デッドマンスイッチ１１０あるいは非常停止スイッチ１１１の回路が開くと、電源１１３からブレーキ１０９への通電が遮断され、モータ１０７は、ブレーキ１０９がかかった状態に保持される。なお、非常停止スイッチ１１１の例としては、教示装置に設けられたスイッチや、ロボット制御装置１００に設けられたスイッチや、マニピュレータの周りに置かれた柵の開閉を検知するリミットスッチ等が挙げられる。また、移動指令生成部１０１は、例えば、ブレーキ１０９に印加される電圧を監視することにより、非常停止指示部１１２が動作したことを検知して、移動指令の送出を即時に停止する。以下、このような一連の動作を非常停止と呼ぶことにする。【００１０】
ここで、マニピュレータを駆動するモータ１０７の回転中に、非常停止が行われた場合、直ちにモータ１０７を制御するサーボ制御を停止すると、モータ１０７が停止するまでの回転量が大きくなる場合がある。すなわち、マニピュレータの惰走距離が大きくなる場合がある。これは、ブレーキ１０９への通電が遮断されてからブレーキ１０９が利き始めるまでのタイムラグや、ブレーキトルクだけでは十分な制動トルクが得られないといったことに起因する。【００１１】
そこで、非常停止が行われた場合、その後もしばらくの間サーボ制御を継続して行う。これにより、モータ１０７に減速トルクを発生させ、併せて、ブレーキ１０９も用いてモータ１０７を停止させる方法が提案されている（例えば、特許文献１参照）。【００１２】
さらには、非常停止時に、例えば、モータ１０７とマニピュレータのアームとを接続する減速機にかかる衝撃を緩和するため、もしくは、モータ１０７と減速機の接続部にかかる衝撃を緩和するため、ブレーキ１０９を、条件に応じて随時、保持したり解除したりするという方法も提案されている（例えば、特許文献２参照）。【００１３】
なお、非常停止時に、サーボ制御を継続してモータ１０７の減速トルクを発生させるための方法として、直接的に減速トルクを発生させる以外に、速度指令をゼロへと誘導するものがある。【００１４】
例えば、偏差カウンタ部１１５内に残存する位置偏差量をゼロクリアして速度指令を直接的にゼロにするもの、あるいは、偏差カウンタ部１１５内に残存する位置偏差量を消化しながら速度指令を漸近的にゼロにしていくもの等である。【００１５】
図１４、図１５は、従来のロボット制御装置における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。【００１６】
図１４は、非常停止時に、図１３に示す偏差カウンタ部１１５内に残存する位置偏差量を消化しながら速度指令をゼロへと誘導する場合の例を示している。すなわち、図１４は、フィードフォワード制御を行う場合の、移動指令と、位置偏差量と、速度指令と、速度指令の微分値の時間的な変化の挙動を示したものである。【００１７】
非常停止後は、図１３の移動指令生成部１０１からの移動指令の送出が停止し、フィードフォワード項がゼロになる。従って、図１４に示すように、非常停止直後の速度指令は、不連続に変化している。なお、この速度指令が不連続に変化している部分におけるその時間微分値（速度指令の加速度）は、理屈上、無限大である。【００１８】
また、図１５は、非常停止時に、図１３に示す偏差カウンタ部１１５内に残存する位置偏差量を消化しないで速度指令をゼロへと誘導する場合の例を示している。すなわち、図１５は、フィードフォワード制御を行わない場合の、移動指令と、位置偏差量と、速度指令およびその速度指令の微分値の時間的な変化の挙動を示したものである。【００１９】
フィードフォワード制御を行わない場合、非常停止直後の速度指令は、図１５に示すように、不連続になることはない。しかし、図１５に示すように、速度指令の微分値は、非常停止直後に瞬間的に大きな値となる。【００２０】
なお、図１４、図１５ともに、速度制御系と電流制御系の応答性は、位置制御系の応答性に比べて十分に高いものとし、位置制御系を時定数が１／Ｋｐである１次遅れ系とした場合を示している。【００２１】
ここで、モータ出力トルクとブレーキトルクを合わせたトルクを総合出力トルクと呼ぶことにすると、非常停止時の減速は、総合出力トルクでなされるということになる。このとき、モータ１０７によって駆動される被駆動部の慣性が、モータ１０７の慣性に比べて十分大きい場合、総合出力トルクの大部分が、モータ１０７と被駆動部とを接続する機構部（例えば、減速機等）にかかる。【００２２】
この総合出力トルクが、機構部の設計強度よりも小さければ、問題はない。しかし、そうでない場合、機構部にダメージを与えることになる。【００２３】
機構部にダメージを与え始める大きさのモータ総合出力トルクを許容最大総合出力トルクと呼ぶことにする。そして、この許容最大総合出力トルクで減速したときの加速度を、限界加速度と呼ぶことにする。速度指令の時間微分値が限界加速度を超えている場合、結果として、機構部に許容最大総合トルクを超えるトルクがかかることになる。これは、非常停止による減速においても、速度制御により、モータ１０７の速度が速度指令通りとなるように制御されるからである。【００２４】
上述したように、非常停止時に、機構部にダメージを与えないようにするためには、減速時の速度指令の微分値を限界加速度より小さく保つ必要がある。しかるに、上述した従来の方法では、そのようなことは考慮されておらず、非常停止時の速度指令の微分値は、必ずしも限界加速度より小さいとは限らない。【００２５】
例えば、図１４に示されるような場合、非常停止直後の速度指令の微分値が限界加速度を超えることは明らかである。また、図１５に示されるような場合でも、非常停止直後の速度指令の微分値が限界加速度よりも小さいという保証はない。【００２６】
すなわち、非常停止時の速度指令の減速のさせ方において、従来の方法では、機構部にダメージを与える可能性があるという課題があった。
【先行技術文献】
【特許文献】
【００２７】
【特許文献１】特開平４−１５２０９１号公報
【特許文献２】特開２００７−１０４８６９号公報
【発明の概要】
【００２８】
本発明は、上記問題を解決するために、機構部にダメージを与えないように非常停止を行うモータ制御装置を提供する。
【００２９】
上記課題を解決するために、本発明のモータ制御装置は、機械の可動部の相対移動を行いブレーキにより停止されるモータの制御を行うモータ制御装置であって、移動指令生成部と、サーボ制御部とを備えている。ここで、移動指令生成部は、モータの移動指令を出力する。サーボ制御部は、上記移動指令生成部の出力に基づいて上記モータを制御するための指令を出力する。上記サーボ制御部は、位置制御部と、速度制御部と、電流制御部と、処理部とを備えている。ここで、位置制御部は、上記移動指令生成部の出力に基づいて速度指令を出力する。速度制御部は、上記位置制御部の出力に基づいて電流指令を出力する。処理部は、上記モータの回転位置を検出する位置検出器の出力に基づいて上記モータの回転角の変化量を出力する。上記位置制御部は、偏差カウンタ部と、第１の係数部と、第２の係数部と、加算部と、偏差補正部と、を備えている。ここで、偏差カウンタ部は、上記移動指令生成部の出力と上記処理部の出力との偏差を求めて出力する。第１の係数部は、上記移動指令生成部の出力を入力して所定の係数を乗じて出力する。第２の係数部は、上記偏差カウンタ部の出力を入力して所定の係数を乗じて出力する。加算部は、上記第１の係数部の出力と上記第２の係数部の出力を加算して上記速度制御部に出力する。上記モータ制御装置および上記モータ制御装置の外部のうちの少なくともいずれかに設けられた非常停止指示部により上記モータの非常停止が指示されると、偏差補正部は、非常停止直前の速度指令である非常停止直前の位置制御周期で上記位置制御部が出力した速度指令値と上記第２の係数部の所定の係数とに基づいて偏差補正値を求める。一方、上記非常停止指示部により非常停止が指示されると、偏差補正部は、上記移動指令生成部による移動指令の出力を停止し、上記偏差カウンタ部の偏差を上記偏差補正部で求めた偏差補正値に置き換えて上記位置制御部による制御を行う構成からなる。
【００３０】
この構成により、非常停止時に、連続性を保ちつつ指定した割合で速度指令を低下させることができ、機構部の強度に合わせた非常停止時の速度指令の減速を行うことができる。これにより、非常停止時の機構部へのダメージを回避することができる。
【図面の簡単な説明】
【００３１】
【図１】図１は、本発明の実施の形態１におけるロボットシステムの概略構成を示す図である。
【図２】図２は、本発明の実施の形態１における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図３】図３は、本発明の実施の形態２におけるロボットシステムの概略構成を示す図である。
【図４】図４は、本発明の実施の形態２における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図５】図５は、本発明の実施の形態３におけるロボットシステムの概略構成を示す図である。
【図６】図６は、本発明の実施の形態３におけるサーボ制御部の処理フローを示す図である。
【図７】図７は、本発明の実施の形態３における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図８】図８は、本発明の実施の形態４におけるロボットシステムの概略構成を示す図である。
【図９】図９は、本発明の実施の形態４におけるサーボ制御部の処理フローを示す図である。
【図１０】図１０は、本発明の実施の形態５におけるロボットシステムの概略構成を示す図である。
【図１１】図１１は、本発明の実施の形態５におけるサーボ制御部の処理フローを示す図である。
【図１２】図１２は、本発明の実施の形態５における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図１３】図１３は、従来のロボットシステムの概略構成を示す図である。
【図１４】図１４は、従来のロボット制御装置における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図１５】図１５は、従来のロボット制御装置における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【発明を実施するための形態】
【００３２】
以下、本発明の一実施の形態について、図面を参照しながら説明する。以下の図面においては、同じ構成要素については同じ符号を付しているので説明を省略する場合がある。
【００３３】
（実施の形態１）
本実施の形態１のモータ制御装置について、図１と図２を用いて説明する。なお、背景技術で説明した図１３と同様の箇所については、詳細な説明は省略する。なお、図１において、図１３と異なる主な点は、位置制御部３内に偏差補正部１８と判定部１９を設けた点である。
【００３４】
図１は、本発明の実施の形態１におけるロボットシステムの概略構成を示す図である。図２は、本発明の実施の形態１における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。なお、ロボットシステムは、モータ７等により駆動されるマニピュレータと、マニピュレータを制御するためのロボット制御装置と、モータ７を停止させるための非常停止指示部１２等から構成される。
【００３５】
また、マニピュレータは、モータ７により駆動されるものであり、モータ７には、位置検出器８やブレーキ９が設けられている。また、モータ制御装置であるロボット制御装置は、移動指令生成部１や、サーボ制御部２や、アンプ６を備えている。また、非常停止指示部１２は、ロボット制御装置に設けても良いし、ロボット制御装置の外部に設けても良いし、ロボット制御装置とロボット制御装置の外部の両方に設けても良い。
【００３６】
移動指令生成部１は、ユーザが作成する動作プログラムに基づいて軌道計画をし、所定の時間毎のモータ７の回転角の変化量を移動指令として生成し、サーボ制御部２へ渡す。
【００３７】
サーボ制御部２は、位置制御部３と、速度制御部４と、電流制御部５と、処理部１４とを備えている。位置制御部３および処理部１４は、位置制御周期毎に制御演算処理を行う。速度制御部４と電流制御部５は、それぞれ所定の周期毎に制御演算処理を行う。
【００３８】
位置制御部３は、移動指令生成部１からの移動指令を受けて位置制御を行い、速度指令を生成する。速度制御部４は、位置制御部３からの速度指令を受けて速度制御を行い、電流指令を生成する。電流制御部５は、速度制御部４からの電流指令を受けて電流制御を行い、電圧指令を生成する。アンプ６は、電流制御部５からの電圧指令を受けてモータ７へ与える電流を生成する。
【００３９】
モータ７には、位置検出器８が接続されており、モータ７の回転角が検出されるようになっている。処理部１４は、位置検出器８から得たモータ７の回転角の位置制御周期当りの変化量を算出し、これを実位置の変化量として出力するものである。
【００４０】
位置制御部３内の偏差カウンタ部１５は、位置制御周期毎に、移動指令生成部１からの移動指令を偏差カウンタ部１５が保持している値に加算し、一方で、処理部１４からの実位置の変化量を偏差カウンタ部１５が保持している値から減算する。そして、このように処理した偏差カウンタ部１５の出力に第２の係数部１６の位置ゲインの値Ｋｐを乗じたものと、移動指令生成部１からの移動指令に第１の係数部１７のフィードフォワード係数の値Ｋｆを乗じたものとを加算部２３で加算し、この和を速度指令として出力する。
【００４１】
また、モータ７には、モータ７を停止する機能を有するブレーキ９が接続されている。このブレーキ９は、ブレーキ９に通電することでブレーキが解除されるようになっている。なお、ブレーキ９に通電しない場合には、ブレーキがかかった状態となる。
【００４２】
電源１３からブレーキ９への通電は、非常停止指示部１２を構成するデッドマンスイッチ１０および非常停止スイッチ１１を介して行われるようになっている。すなわち、デッドマンスイッチ１０もしくは非常停止スイッチ１１の回路が開いた状態になると、電源１３からブレーキ９への通電が遮断され、ブレーキが保持された状態、すなわち、ブレーキがかかった状態となる。
【００４３】
また、ブレーキ９への通電が遮断されたことは、移動指令生成部１で検知されるようになっている。なお、移動指令生成部１は、例えば、ブレーキ９に印加される電圧を監視することで、ブレーキ９への通電が遮断されたことを検知する。移動指令生成部１は、ブレーキ９への通電が遮断されたことを検知した場合、即座に移動指令の送出を停止する。
【００４４】
なお、以下の説明において、非常停止とは、デッドマンスイッチ１０もしくは非常停止スイッチ１１の回路が開いてブレーキ９が保持され、移動指令生成部１からの移動指令の送出が停止されることを指すものとする。
【００４５】
ここで、サーボ制御部２は、非常停止されたことを次のようにして検知する。サーボ制御部２内には、判定部１９が設けられている。この判定部１９は、移動指令生成部１から位置制御部３に入力される移動指令を監視し、位置制御周期毎に定期的に送られてくる移動指令が中断された場合、非常停止されたと判断する。判定部１９は、適宜、サーボ制御部２内の非常停止情報を必要とする偏差補正部１８に、非常停止されたことを通知する。なお、移動指令生成部１からサーボ制御部２内の判定部１９へ、直接的に非常停止したことを通知して非常停止されたことを判断するようにしてもよい。
【００４６】
偏差補正部１８は、判定部１９から非常停止が行われたことを検知した旨の信号を受け取った場合、非常停止直前の速度指令値Ｖａ（前回の位置制御周期で位置制御部３が出力した速度指令値）と第２の係数部１６の位置ゲインの値Ｋｐとを用いて、下記（数１）により置換量Δθｃを求める。なお、置換量Δθｃは、後述するように、非常停止の前後で速度指令が不連続とならず連続となるようにするために用いられる値である。
【００４７】
【数１】
2013005388.tif 000003
【００４８】
そして、偏差カウンタ部１５の値を、この置換量Δθｃで置き換える。
【００４９】
これらの操作は、非常停止が検知されて以後の最初の位置制御周期で１回だけ行う。そして、それ以後もサーボ制御処理は継続して行う。
【００５０】
すなわち、偏差カウンタ部１５の偏差の偏差補正部１８で求めた偏差補正値への置き換えは、非常停止が行われた直後の最初の位置制御周期で１回だけ行い、それ以後は、偏差カウンタ部１５の値と処理部１４の出力との偏差に基づいて制御を行う構成としてもよい。この構成により、移動指令生成部１からの移動指令が停止された状態でも、偏差カウンタ部１５の値と処理部１４の出力に基づいてサーボ制御処理が継続して行われる。これにより、偏差カウンタ部１５の値が消化されてゼロとなり、すなわち、速度指令がゼロとなり、モータ７が完全に停止することとなる。
【００５１】
次に、図２に、図１の構成を用いたモータ制御装置の非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す。図２は、それぞれ、非常停止前後における、移動指令、位置偏差量Δθ（ｔ）、速度指令Ｖｃｍｄ（ｔ）、速度指令の微分値Ｖｃｍｄ’（ｔ）の時間的な変化を示している。なお、速度指令の微分値Ｖｃｍｄ’（ｔ）では、図を見やすくするため、符号を反転して記載している。図２の移動指令の時間的な変化は、非常停止したことにより移動指令の送出が停止したことを示している。なお、図２に示すように、位置偏差量Δθ（ｔ）は、偏差カウンタ部１５に保持される偏差カウンタ部値のことであり、Δθｒは非常停止直前の偏差カウンタ部の値である。また、図２の速度指令の時間的な変化において、点線で表している直線および曲線は、参考として示す従来の方法の場合の速度指令の挙動を示しており、Ｋｆ・Ｗａはフィードフォワード項に当たる。
【００５２】
図２の速度指令Ｖｃｍｄ（ｔ）の時間的な変化に関し、従来の場合は、点線の曲線で示すように、移動指令の送出が停止した直後に不連続に変化するのに対して、偏差カウンタ値を（数１）で算出した置換量Δθｃに置き換える。これにより、以下で説明するように、非常停止の直前と直後で速度指令を同じ値とすることができ、速度指令が連続的につながるようになる。
【００５３】
以下、詳細について説明する。
【００５４】
速度制御部４によって制御される速度制御系および電流制御部５によって制御される電流制御系の応答が、一般的に知られているように、位置制御部３によって制御される位置制御系の応答に比べて十分に高いものであるとする。そうすると、位置制御系は、１／Ｋｐを時定数とする１次遅れ系と見なすことができる。よって、移動指令停止時点での時間をｔ＝０として、位置偏差量の時間関数をΔθ（ｔ）とすれば、移動指令停止以後の位置偏差量Δθ（ｔ）は、下記（数２）のようになる。
【００５５】
【数２】
2013005388.tif 000004
【００５６】
また、移動指令停止後の速度指令Ｖｃｍｄ（ｔ）は、フィードフォワード項が０であるので、（数３）のようになる。
【００５７】
【数３】
2013005388.tif 000005
【００５８】
ｔ＝０における速度指令Ｖｃｍｄ（０）は、上記（数３）、上記（数１）より、
【００５９】
【数４】
2013005388.tif 000006
【００６０】
（数３）はｔ≧０において連続であること、および（数４）より、ｔ＝０において速度指令Ｖｃｍｄ（ｔ）は連続である。
【００６１】
また、速度指令Ｖｃｍｄ（ｔ）の時間微分値Ｖｃｍｄ’（ｔ）は、下記（数５）のようになる。
【００６２】
【数５】
2013005388.tif 000007
【００６３】
（数５）より、非常停止直後のＶｃｍｄ’（ｔ）の絶対値は、ｔ＝０において最大の値をとり（数６）となる。
【００６４】
【数６】
2013005388.tif 000008
【００６５】
上記（数６）で算出される値が限界加速度を超えていなければ、モータの総合出力トルクは、許容最大総合出力トルク以下となり、機構部にダメージを与えることはない。
【００６６】
なお、総合出力トルクとは、モータ出力トルクとブレーキトルクを合わせたトルクのことであり、モータ７を停止するためのトルクのことである。また、許容最大総合出力トルクとは、機構部にダメージを与え始める大きさのモータ総合トルクのことである。また、限界加速度とは、この許容最大総合出力トルクで減速したときの加速度のことである。
【００６７】
以上のように、非常停止時に偏差カウンタ部１５の値を置き換えることにより、非常停止時にも、速度指令を不連続とせず、連続とすることができる。
【００６８】
これにより、非常停止時に機構部へ過大なトルクがかかるのを防止することができ、非常停止時の機構部へのダメージを回避することができる。
【００６９】
すなわち、本発明のモータ制御装置は、機械の可動部の相対移動を行いブレーキ９により停止されるモータ７の制御を行うモータ制御装置であって、移動指令生成部１と、サーボ制御部２とを備えている。ここで、移動指令生成部１は、モータ７の移動指令を出力する。サーボ制御部２は、移動指令生成部１の出力に基づいてモータ７を制御するための指令を出力する。サーボ制御部２は、位置制御部３と、速度制御部４と、電流制御部５と、処理部１４とを備えている。ここで、位置制御部３は、移動指令生成部１の出力に基づいて速度指令を出力する。速度制御部４は、位置制御部３の出力に基づいて電流指令を出力する。処理部１４は、モータ７の回転位置を検出する位置検出器８の出力に基づいてモータ７の回転角の変化量を出力する。位置制御部３は、偏差カウンタ部１５と、第１の係数部１７と、第２の係数部１６と、加算部２３と、偏差補正部１８と、を備えている。ここで、偏差カウンタ部１５は、移動指令生成部１の出力と処理部１４の出力との偏差を求めて出力する。第１の係数部１７は、移動指令生成部１の出力を入力して所定の係数を乗じて出力する。第２の係数部１６は、偏差カウンタ部１５の出力を入力して所定の係数を乗じて出力する。加算部２３は、第１の係数部１７の出力と第２の係数部１６の出力を加算して速度制御部４に出力する。モータ制御装置およびこのモータ制御装置の外部のうちの少なくともいずれかに設けられた非常停止指示部１２によりモータ７の非常停止が指示されると、偏差補正部１８は、非常停止直前の速度指令である非常停止直前の位置制御周期で位置制御部３が出力した速度指令値と第２の係数部１６の所定の係数とに基づいて偏差補正値を求める。一方、非常停止指示部１２により非常停止が指示されると、偏差補正部１８は、移動指令生成部１による移動指令の出力を停止し、偏差カウンタ部１５の偏差を偏差補正部１８で求めた偏差補正値に置き換えて位置制御部３による制御を行う構成からなる。
【００７０】
この構成により、非常停止時に、連続性を保ちつつ指定した割合で速度指令を低下させることができ、機構部の強度に合わせた非常停止時の速度指令の減速を行うことができる。これにより、非常停止時の機構部へのダメージを回避することができる。
【００７１】
（実施の形態２）
本実施の形態２のモータ制御装置について、図３と図４を用いて説明する。図３は、本発明の実施の形態２におけるロボットシステムの概略構成を示す図である。図４は、本発明の実施の形態２における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【００７２】
実施の形態１で示したモータ制御装置の構成により、非常停止時も、速度指令が不連続になることを防止し、速度指令の微分値を下げることができる。しかし、例えば、第２の係数部１６の位置ゲインの値Ｋｐが大きい場合、限界加速度を超える場合がある。そこで、これに対応するための別の例として、実施の形態２のモータ制御装置について説明する。
【００７３】
本実施の形態２のロボットシステムの概略構成が実施の形態１と異なる主な点は、実施の形態１で説明した図１の構成に、係数補正部２２を加えた点である。
【００７４】
係数補正部２２は、後述のように値Ｋｐｃを求め、第２の係数部１６の位置ゲインの値Ｋｐを値Ｋｐｃに置き換える処理を行うものである。
【００７５】
判定部１９により非常停止であることが検知された場合、係数補正部２２は、値Ｋｐｃを次のようにして求める。すなわち、限界加速度の値をＡｃ（＞０）とし、非常停止の直前の位置制御部２１が出力する速度指令値をＶａとして、下記（数７）で演算して求める。
【００７６】
【数７】
2013005388.tif 000009
【００７７】
そして、第２の係数部１６の位置ゲインの値をこのＫｐｃに置き換える。
【００７８】
すなわち、本実施の形態２のモータ制御装置は、係数の補正値を求める係数補正部２２を備え、非常停止指示部１２により非常停止が指示されると、第２の係数部１６の所定の係数を係数補正部２２で求めた係数補正値に置き換えて位置制御部２１による制御を行う構成としてもよい。ここで、係数補正部２２は、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、非常停止が指示される直前の位置制御部２１が出力する速度指令値とに基づいて、係数の補正値を求めている。この構成により、非常停止時に、連続性を保ちつつ指定した割合で速度指令を低下させることができ、機構部の強度に合わせた非常停止時の速度指令の減速を行うことができる。これにより、非常停止時の機構部へのダメージを回避することができる。
【００７９】
次に、偏差補正部１８において、この位置ゲインの値Ｋｐｃと速度指令値Ｖａとから、置換量Δθｃを下記（数８）により求める。
【００８０】
【数８】
2013005388.tif 000010
【００８１】
そして、偏差カウンタ部１５の値をこの置換量Δθｃに置き換える。
【００８２】
なお、係数補正部２２および偏差補正部１８によるこれらの操作は、非常停止が検知されて以後の最初の位置制御周期で１回だけ行う。そして、サーボ制御部２０は、それ以後も置き換えた係数補正値を用いて制御を行う。これにより、サーボ制御処理は継続して行われる。
【００８３】
すなわち、本実施の形態２のモータ制御装置において、第２の係数部１６の所定の係数の係数補正部２２で求めた係数補正値への置き換えは、非常停止が行われた直後の最初の位置制御周期で１回だけ行い、それ以後は、置き換えた係数補正値を用いて制御を行う構成としてもよい。この構成により、移動指令生成部１からの移動指令が停止された状態でも、偏差カウンタ部１５の値と処理部１４の出力に基づいてサーボ制御処理が継続して行われる。これにより、偏差カウンタ部１５の値が消化されてゼロとなり、すなわち、速度指令がゼロとなり、モータ７が完全に停止することとなる。
【００８４】
係数補正部２２および偏差補正部１８により、これらの操作を行ったときの、非常停止直後の速度指令の微分値は、上記（数６）と（数７）より、
｜Ｖｃｍｄ’（０）｜＝｜Ｋｐｃ・Ｖａ｜＝Ａｃ
となる。
【００８５】
なお、限界加速度Ａｃの値は、機構部の強度に合わせて予め決めておいた値とする。
【００８６】
次に、図４に、図３のモータ制御装置の構成を用いたときの、非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す。図４の時間的な変化は、上からそれぞれ、非常停止前後における、移動指令、位置偏差量Δθ（ｔ）、速度指令Ｖｃｍｄ（ｔ）および速度指令の微分値Ｖｃｍｄ’（ｔ）を示している。図４は、図２で示したものと同じ物理量のものを示す。
【００８７】
図４には、図３に示す係数補正部２２により位置ゲインの値がＫｐからＫｐｃへと変更されても、速度指令Ｖｃｍｄ（ｔ）は、連続的につながることが示されている。
【００８８】
また、図４には、位置ゲインの値を上記（数７）のようにして決めることで、速度指令の微分値は、限界加速度以下に抑えられることが示されている。
【００８９】
以上のように、非常停止時に、偏差カウンタ部１５の値および第２の係数部１６の位置ゲインの値を置き換えることにより、連続性を保ちつつ指定した割合で速度指令を低下させることができ、限界加速度以下とすることができる。これにより、機構部の強度に合わせた、非常停止時の速度指令減速を行うことができ、非常停止時の機構部へのダメージを回避することができる。
【００９０】
（実施の形態３）
本実施の形態３のモータ制御装置について、図５から図７を用いて説明する。図５は、本発明の実施の形態３におけるロボットシステムの概略構成を示す図である。図６は、発明の実施の形態３におけるサーボ制御部３０の処理フローを示す図である。図７は、本発明の実施の形態３における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【００９１】
本実施の形態３のモータ制御装置が実施の形態１と異なる主な点は、実施の形態１で説明した図１の構成に、偏差更新部３２を加えた点である。なお、偏差更新部３２は、位置制御周期毎に偏差カウンタ部１５の値を補正する部分である。
【００９２】
判定部１９により非常停止が検知された場合、まず、偏差補正部１８で、非常停止直前の速度指令値Ｖａ（前回の位置制御周期で位置制御部３１が出力した速度指令値）と第２の係数部１６の位置ゲインの値Ｋｐとを用いて、実施の形態１で示した（数１）により置換量Δθｃを求める。そして、偏差カウンタ部１５の値をこの置換量Δθｃに置き換える。なお、これらの操作は、非常停止後に１回だけ行う。
【００９３】
次に、詳細は後述するが、偏差更新部３２により、偏差カウンタ部１５の値を位置制御周期毎に補正していく。なお、偏差更新部３２の処理は、非常停止が検知されて以後に行う。
【００９４】
偏差更新部３２による補正を含む詳細な処理について、図６を用いて説明する。
【００９５】
なお、図６に示すステップＳ１とステップＳ２は、非常停止を検知して以後に一度だけ行い、ステップＳ３を含むそれ以後のステップは、位置制御周期毎に行う。
【００９６】
ステップＳ１は、上述の偏差補正部１８における処理である。
【００９７】
ステップＳ１の処理を行った後、ステップＳ２において、非常停止直前の速度指令値をＶａ、限界加速度をＡｃ、位置制御周期をＴｐとして、下記（数９）により、位置偏差量Δθ（ｔ）の減少の割合を一定にするための制限量Ｄを求める。
【００９８】
【数９】
2013005388.tif 000011
【００９９】
ここで、制限量Ｄは、符号付きの量である。
【０１００】
ステップ３において、非常停止を指示してから所定時間が経過していなければ、ステップＳ４、さらにステップＳ５へと進む。なお、非常停止を指示してから所定時間が経過した場合については後述する。
【０１０１】
ステップＳ４およびステップＳ５は、実施の形態１において図１を用いて説明した位置制御演算に相当する。すなわち、ステップＳ４では、位置制御周期当りの実位置変化量を求め、ステップＳ５では、偏差カウンタ部１５から実位置変化量を差し引いて偏差カウンタ部１５の値を更新する。
【０１０２】
なお、以下では、実位置変化量を差し引く前の偏差カウンタ部１５の位置偏差カウント値を、「前回偏差」といい、実位置変化量を差し引いた後の位置偏差カウンタ値を「今回偏差」ということにする。
【０１０３】
ステップＳ７において、ステップＳ２で求めた制限量Ｄを前回偏差から差し引いた値の絶対値と、ステップＳ５で求めた今回偏差の絶対値との比較をする。そして、前者の方が大きければ、ステップＳ８において、今回偏差を、前回偏差から制限量Ｄを差し引いた値で置き換える。
【０１０４】
ステップＳ１１において、上記のように補正した今回偏差に、位置ゲインの値Ｋｐを乗じて、速度指令とする。
【０１０５】
さらに、この速度指令に基づいて、ステップＳ１２として、速度制御部４による速度制御と、電流制御部５による電流制御を行う。なお、速度制御と電流制御は、位置制御周期当りに１回とは限らず、複数回行われることもある。
【０１０６】
また、ステップＳ１３のように、今回偏差は、次の位置制御周期における前回偏差となる。
【０１０７】
なお、ステップＳ３に示すように、非常停止から所定時間が経過した場合は、サーボ制御を停止するようにしている。しかし、サーボ制御を停止せず、継続するようにしてもかまわない。その場合は、偏差カウンタ部が０になるように位置制御がなされる。
【０１０８】
次に、図７に、図５のモータ制御装置の構成を用いたときの、非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す。図７の時間的な変化は、上からそれぞれ、非常停止前後における、移動指令、位置偏差量Δθ（ｔ）、速度指令Ｖｃｍｄ（ｔ）および速度指令の微分値Ｖｃｍｄ’（ｔ）を示している。図７は、図２で示したものと同じ物理量のものを示す。
【０１０９】
図７に示す位置偏差量Δθ（ｔ）は、実速度（実位置の変化量）の大きさに応じた割合で減少していこうとする。しかし、ステップＳ５からステップＳ１１までの処理により、位置偏差量Δθ（ｔ）の減少の割合には制限がかかる。結果として、位置偏差量Δθ（ｔ）は、１次関数の形で減少していく。よって、図７に示す速度指令Ｖｃｍｄ（ｔ）も１次関数の形で減少していき、その微分値は一定値となる。
【０１１０】
具体的には、非常停止後の位置偏差量Δθ（ｔ）と、速度指令値Ｖｃｍｄ（ｔ）と、速度指令の微分値Ｖｃｍｄ’（ｔ）は、それぞれ、下記（数１０）、（数１１）、（数１２）のようになる。
【０１１１】
【数１０】
2013005388.tif 000012
【０１１２】
【数１１】
2013005388.tif 000013
【０１１３】
【数１２】
2013005388.tif 000014
【０１１４】
さらに、（数１２）と（数１）、（数９）より、下記（数１３）を得る。
【０１１５】
【数１３】
2013005388.tif 000015
【０１１６】
なお、速度指令が低下していって、｜Ｔｐ・Ｖｃｍｄ（ｔ）｜≦｜制限量Ｄ｜となる区間（ｔ≧ｔ１）では、位置偏差量Δθ（ｔ）は、その減少に制限がかからなくなり、図２または図４で示したものと同じ指数関数の形となる。よって、速度指令Ｖｃｍｄ（ｔ）も同様の形となり、また、その微分値も限界加速度Ａｃよりも小さい値となる。
【０１１７】
すなわち、本実施の形態３のモータ制御装置は、非常停止指示部１２により非常停止が指示されて偏差カウンタ部１５の偏差を偏差補正部１８で求めた偏差補正値に置き換えた後に、偏差カウンタ部１５の値を位置制御周期毎に補正する偏差更新部３２を備えている。そして、偏差更新部３２は、非常停止直前の速度指令値と、限界加速度と、位置制御周期と、偏差補正値と、前回偏差と、に基づいて、今回偏差を求め、位置制御周期毎に偏差カウンタ部１５の偏差を求めた今回偏差に置き換えて制御を行う構成としてもよい。ここで、限界加速度は、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である。偏差補正値は、偏差補正部１８で求められる。前回偏差は、処理部１４の出力である実位置変化量を差し引く前の偏差カウンタ部１５の値である。今回偏差は、位置制御周期毎に実位置変化量を差し引いた後の偏差カウンタ部１５の値として求められる。
【０１１８】
この構成により、非常停止時に偏差カウンタ部１５の値を置き換え、さらに、偏差カウンタ部１５の値を補正しながら位置制御を行うことで、速度指令を、連続性を保ちつつ一定の割合で低下させることができる。これにより、非常停止時の機構部へのダメージを回避しながら、不必要に減速距離を伸ばすことなく減速停止させることができる。
【０１１９】
（実施の形態４）
本実施の形態４のモータ制御装置について、図８と図９を用いて説明する。図８は、本発明の実施の形態４におけるロボットシステムの概略構成を示す図である。図９は、本発明の実施の形態４におけるサーボ制御部４０の処理フローを示す図である。
【０１２０】
本実施の形態４のロボットシステムの概略構成が実施の形態１と異なる主な点は、実施の形態１で説明した図１の構成に、第１の位置補正部４２を加えた点である。なお、第１の位置補正部４２は、位置制御周期毎に偏差カウンタ部１５から差し引く実位置変化量を補正する部分である。
【０１２１】
判定部１９により非常停止が検知されたら、まず偏差補正部１８で、非常停止直前の速度指令値Ｖａ（前回の位置制御周期で位置制御部４１が出力した速度指令値）と第２の係数部１６の位置ゲインの値Ｋｐとを用いて、実施の形態１で説明した（数１）により置換量Δθｃを求める。そして、偏差カウンタ部１５の値を、この置換量Δθｃで置き換える。なお、これらの操作は非常停止後に１回だけ行う。
【０１２２】
次に、詳細は後述するが、第１の位置補正部４２において、偏差カウンタ部１５から差し引く実位置変化量の値を位置制御周期毎に補正していく。なお、第１の位置補正部４２の処理は、非常停止が検知されて以後に行う。
【０１２３】
第１の位置補正部４２による補正を含む詳細な処理について、図９を用いて説明する。なお、図９に示すように、実施の形態３で用いた図６と同じ処理を行うステップについては、図６と同じ符号を付している。また、図９に示すステップＳ１とステップＳ２は、非常停止を検知して以後に一度だけ行い、ステップＳ３を含むそれ以後のステップは、位置制御周期毎に行う。
【０１２４】
ステップＳ１は、上述の偏差補正部１８における処理である。ステップＳ１の処理を行った後、ステップＳ２において、非常停止直前の速度指令値Ｖａと、限界加速度Ａｃと、位置制御周期Ｔｐとを用いて、実施の形態３で示した（数９）により制限量Ｄを求める。ここで、制限量Ｄは、符号付きの量である。
【０１２５】
ステップＳ３において、非常停止してから所定時間が経過していなければ、ステップＳ４以降へと進む。ステップＳ４において、位置制御周期当りの実位置変化量を求める。この後は、図９に示すようにステップＳ２０に進む。なお、非常停止してから所定時間が経過した場合については後述する。
【０１２６】
次に、ステップＳ２０において、ステップＳ４で求めた実位置変化量を位置フィードバック量とする。なお、位置フィードバック量は、図９のステップＳ２０では、位置ＦＢ量と記載している。
【０１２７】
ステップＳ２１において、位置フィードバック量の絶対値と制限量Ｄの絶対値との比較を行い、位置フィードバック量の絶対値の方が大きければ、ステップＳ２３において、位置フィードバック量を制限量Ｄで置き換える。
【０１２８】
なお、ステップＳ２１における判定で、位置フィードバック量の絶対値の大きさが制限量Ｄの絶対値より大きくない場合、位置フィードバック量の置き換えは行わない。
【０１２９】
ステップＳ２５において、上記のようにして求めた位置フィードバック量を、前回偏差から差し引いた値を、今回偏差とする。ここで、今回偏差、前回偏差の意味は、実施の形態３で説明したものと同じである。
【０１３０】
次に、ステップＳ１１において、今回偏差に位置ゲインの値Ｋｐを乗じて速度指令を算出する。そして、ステップＳ１２において速度制御および電流制御を行う。さらに、ステップＳ１３において、今回偏差を次回の前回偏差として記憶し、ステップＳ３へと戻る。
【０１３１】
なお、図８の構成を用いたときの非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動は、実施の形態３で説明した図７と同じになるので、説明は省略する。
【０１３２】
本実施の形態４のモータ制御装置は、位置制御周期毎に処理部１４の出力を補正して偏差カウンタ部１５に出力する第１の位置補正部４２を備えている。この第１の位置補正部４２は、処理部１４で求めた実位置変化量と、非常停止直前の速度指令値と、限界加速度と、位置制御周期と、偏差補正部１８で求めた偏差補正値に基づいて、偏差カウンタ部１５に出力する位置フィードバック量を求める構成としてもよい。ここで、限界加速度は、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である。
【０１３３】
この構成により、非常停止時に偏差カウンタ部１５の値を置き換え、さらに、偏差カウンタ部１５への位置フィードバック量を補正しながら位置制御を行う。そうすると、速度指令を、連続性を保ちつつ一定の割合で低下させることができる。これにより、非常停止時の機構部へのダメージを回避しながら、不必要に減速距離を伸ばすことなく減速停止させることができる。
【０１３４】
（実施の形態５）
本実施の形態５のモータ制御装置について、図１０から図１２を用いて説明する。図１０は、本発明の実施の形態５におけるロボットシステムの概略構成を示す図である。図１１は、本発明の実施の形態５におけるサーボ制御部５０の処理フローを示す図である。図１２は、本発明の実施の形態５における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【０１３５】
本実施の形態５のモータ制御装置が実施の形態１と異なる主な点は、実施の形態１で説明した図１の構成に、第２の位置補正部５２を加えた点である。なお、この第２の位置補正部５２は、図８を用いて実施の形態４で説明した第１の位置補正部４２とは異なるものである。
【０１３６】
第２の位置補正部５２は、位置制御周期毎に偏差カウンタ部１５から差し引く実位置変化量を補正する部分である。第２の位置補正部５２は、位置制御周期毎に実位置変化量を積算していったものを位置積算量として保持する。そして、第２の位置補正部５２は、速度指令がゼロになるまで、偏差カウンタ部１５の減少の割合を一定にするために、位置積算量の中から位置制御周期毎に一定量を取り出して位置フィードバックとして出力する。
【０１３７】
本実施の形態５のモータ制御装置において、非常停止指示部１２により非常停止が指示されると、実位置変化量の積算量をゼロにした後に実位置変化量の積算を行い、実位置変化量の積算量をゼロにする処理は、前記非常停止がされたことを検知して以後一度だけ行われる構成としてもよい。これにより、非常停止時の機構部へのダメージを回避しながら、必要かつ最短の減速距離で減速停止させることができる。
【０１３８】
判定部１９により非常停止が検知された場合、まず、偏差補正部１８で、非常停止直前の速度指令値Ｖａ（前回の位置制御周期で位置制御部５１が出力した速度指令値）と第２の係数部１６の位置ゲインの値Ｋｐとを用いて、実施の形態１で説明した（数１）により置換量Δθｃを求める。そして、偏差カウンタ部１５の値をこの置換量Δθｃで置き換える。なお、これらの操作は非常停止後に１回だけ行う。
【０１３９】
次に、第２の位置補正部５２で、位置制御周期毎に処理部１４から入力した実位置変化量を補正処理して位置フィードバック量を算出する。そして、位置制御周期毎に偏差カウンタ部１５からこの位置フィードバック量を差し引く。なお、第２の位置補正部５２の処理は、非常停止が検知されて以後に行う。また、第２の位置補正部５２内の位置積算量は、非常停止が検知されて以後の最初の位置制御周期に１回だけゼロクリアされる。
【０１４０】
第２の位置補正部５２による補正方法を含む詳細な処理のフローを、図１１に示す。なお、図１１において、実施の形態３で説明した図６や、実施の形態４で説明した図９と同じ処理を行うステップについては、図６や図９で用いたものと同じ符号を付している。なお、図１１におけるステップＳ１、ステップＳ２およびステップＳ３０の処理は、非常停止を検知して以後に一度だけ行う。そして、ステップＳ３を含むそれ以後のステップは、位置制御周期毎に行う。
【０１４１】
ステップＳ１は、上述の偏差補正部１８の処理である。ステップＳ１の処理を行った後、ステップＳ２において、非常停止直前の速度指令値Ｖａと、限界加速度Ａｃと、位置制御周期Ｔｐを用いて、実施の形態３で示した（数９）により制限量Ｄを求める。ここで、制限量Ｄは、符号付きの量である。
【０１４２】
図１０に示す第２の位置補正部５２には、位置積算量が含まれ、ステップＳ３０ではこれをゼロクリアする。ステップＳ３において、非常停止してから所定時間が経過していなければ、ステップＳ４以降へと進み、ステップＳ４で位置制御周期当りの実位置変化量を求める。
【０１４３】
次に、ステップＳ３１において、ステップＳ４で求めた実位置変化量を、位置積算量に加算する。ステップＳ３２において、ステップＳ３１で求めた位置積算量の絶対値とステップＳ２で求めた制限量Ｄの絶対値との比較を行う。位置積算量の絶対値の方が大きければ、ステップＳ２３において、位置フィードバック量を制限量Ｄで置き換える。
【０１４４】
ステップＳ３２における判定で、位置積算量の絶対値の大きさが制限量Ｄの絶対値より大きくない場合は、ステップＳ３４において、位置フィードバック量は位置積算量とする。
【０１４５】
このようにして位置フィードバック量（図１１では、「位置ＦＢ量」とする）を求め、ステップＳ３５において、位置積算量から位置フィードバック量を差し引いて、次回の位置積算量とする。また、ステップＳ２５において、前回偏差から位置フィードバック量を差し引いて今回偏差を求める。
【０１４６】
次に、ステップＳ１１において、今回偏差に位置ゲインの値Ｋｐを乗じて速度指令を算出する。そして、ステップＳ１２において、速度制御および電流制御を行い、ステップＳ１３において、今回偏差を次回の前回偏差として記憶し、ステップＳ３へと戻る。
【０１４７】
次に、図１２に、図１０のモータ制御装置の構成を用いたときの非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す。図１２の時間的な変化は、上からそれぞれ、非常停止前後における、移動指令、位置偏差量Δθ（ｔ）、速度指令Ｖｃｍｄ（ｔ）および速度指令の微分値Ｖｃｍｄ’（ｔ）を示している。図１２は、図２で示したものと同じ物理量のものを示す。
【０１４８】
位置偏差量Δθ（ｔ）は、実速度（実位置の変化量）の大きさに応じた割合で減少していこうとする。しかし、ステップＳ３１からステップＳ３５までの処理により、位置偏差量Δθ（ｔ）の減少の割合には制限がかかる。結果として位置偏差量Δθ（ｔ）は１次関数の形で減少していく。よって、速度指令Ｖｃｍｄ（ｔ）も１次関数の形で減少していき、その微分値は一定値となる。
【０１４９】
具体的には、非常停止後の全区間にわたって、実施の形態３で示した（数１０）から（数１３）までの式で示される形となる。
【０１５０】
なお、速度が低下していって、｜Ｔｐ・Ｖｃｍｄ（ｔ）｜≦制限量Ｄとなって以後も、｜位置積算量｜＞制限量Ｄであるので、位置偏差量がゼロになる直前までは、（数１０）から（数１３）までの式で表される状態が保たれる。
【０１５１】
すなわち、位置偏差量がゼロになる直前までの全域にわたって、速度指令の微分値は一定であり、限界加速度Ａｃに保たれる。
【０１５２】
すなわち、本実施の形態５のモータ制御装置は、位置制御周期毎に処理部１４の出力を補正して偏差カウンタ部１５に出力する第２の位置補正部５２を備えている。そして、第２の位置補正部５２は、処理部１４で求めた実位置変化量を位置制御周期毎に積算した積算量と、非常停止直前の速度指令値と、限界加速度と、位置制御周期と、偏差補正部１８で求めた偏差補正値に基づいて、偏差カウンタ部１５に出力する位置フィードバック量を求める構成としてもよい。ここで、限界加速度は、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である。
【０１５３】
この構成により、非常停止時に、偏差カウンタ部１５の値を置き換え、さらに、偏差カウンタ部１５への位置フィードバック量を一定に保ちながら、位置制御を行う。そうすると、速度指令を、連続性を保ちつつ一定の割合で低下させることができる。これにより、非常停止時の機構部へのダメージを回避しながら、最短の減速距離で減速停止させることができる。
【産業上の利用可能性】
【０１５４】
本発明のモータ制御装置は、非常停止時の減速で機構部に過大なトルクがかかるのを防止することができ、機構部へのダメージを回避できるので非常停止を行うシステム等に用いるモータ制御装置として産業上有用である。
【符号の説明】
【０１５５】
１    移動指令生成部
２，２０，３０，４０，５０    サーボ制御部
３，２１，３１，４１，５１    位置制御部
４    速度制御部
５    電流制御部
６    アンプ
７    モータ
８    位置検出器
９    ブレーキ
１０    デッドマンスイッチ
１１    非常停止スイッチ
１２    非常停止指示部
１３    電源
１４    処理部
１５    偏差カウンタ部
１６    第２の係数部
１７    第１の係数部
１８    偏差補正部
１９    判定部
２２    係数補正部
２３    加算部
３２    偏差更新部
４２    第１の位置補正部
５２    第２の位置補正部
【図１】
2013005388.tif 000016
【図２】
2013005388.tif 000017
【図３】
2013005388.tif 000018
【図４】
2013005388.tif 000019
【図５】
2013005388.tif 000020
【図６】
2013005388.tif 000021
【図７】
2013005388.tif 000022
【図８】
2013005388.tif 000023
【図９】
2013005388.tif 000024
【図１０】
2013005388.tif 000025
【図１１】
2013005388.tif 000026
【図１２】
2013005388.tif 000027
【図１３】
2013005388.tif 000028
【図１４】
2013005388.tif 000029
【図１５】
2013005388.tif 000030
【手続補正書】
【提出日】平成24年11月1日(2012.11.1)
【手続補正１】
【補正対象書類名】明細書
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置等のモータ制御装置を用いたシステムにおける非常停止時のモータの停止制御に関する。
【背景技術】
【０００２】
ロボット制御装置等のモータ制御装置を使ったロボットシステムの一例について、図１３を用いて説明する。なお、ロボットシステムは、モータ等により駆動されるマニピュレータと、マニピュレータを制御するためのロボット制御装置等から構成される。
【０００３】
図１３は、従来のロボットシステムの概略構成を示す図である。図１３に示すロボットシステムを構成するロボット制御装置１００は、移動指令生成部１０１と、サーボ制御部６０と、アンプ１０６と、を備えている。ここで、移動指令生成部１０１は、ユーザが作成する動作プログラムに基づいて軌道計画をして移動指令を生成する。サーボ制御部６０は、移動指令生成部１０１が出力した移動指令を受けてモータ１０７を駆動制御する。アンプ１０６は、サーボ制御部６０の出力に基づいてモータ１０７を制御する。
【０００４】
図１３に示すように、サーボ制御部６０は、位置制御部６１と、速度制御部１０４と、電流制御部１０５と、処理部１１４と、を備えている。これら各制御部は、一定周期ごとに制御演算を行うデジタル制御系で構成される。
【０００５】
位置制御部６１は、移動指令生成部１０１からの移動指令を受けて位置制御を行い、速度指令を生成する。速度制御部１０４は、位置制御部６１からの速度指令を受けて速度制御を行い、電流指令を生成する。電流制御部１０５は、速度制御部１０４からの電流指令を受けて電流制御を行い、電圧指令を生成する。アンプ１０６は、電流制御部１０５からの電圧指令を受けてモータ１０７に与えるモータ電流を生成する。
【０００６】
移動指令生成部１０１からサーボ制御部６０へ渡される移動指令は、所定の時間毎のモータ１０７の回転角の変化量の形をとる。位置制御部６１は、偏差カウンタ部１１５を構成要素としている。位置制御部６１では、位置制御周期毎に、移動指令を偏差カウンタ部１１５に加算し、一方で、処理部１１４の出力である実位置の位置制御周期毎のモータ１０７の実際の回転角の変化量を偏差カウンタ部１１５から減算する。なお、位置制御周期毎のモータ１０７の回転角の変化量は、モータ１０７に付随する位置検出器１０８が検出したモータ１０７の回転角に基づいて処理部１１４において算出されるものである。このように処理した偏差カウンタ部１１５の偏差カウンタ値に対し、位置ゲイン１１６の係数を乗じて速度指令とする。さらに、これに加えて、移動指令生成部１０１からの移動指令にフィードフォワード係数１１７の係数を乗じた値を速度指令の一部として加算するフィードフォワード制御を備えている。すなわち、位置ゲイン１１６の出力とフィードフォワード係数１１７の出力とを併せたものが、位置制御部６１が出力する速度指令となる。
【０００７】
また、モータ１０７には、励磁制御を行わないときに外力で不用意に動いてしまうことを防ぐためのブレーキ１０９が備えられている。このブレーキ１０９は、通常は、保持されてブレーキがかかった状態であり、電流を通じることでブレーキが解除される。
【０００８】
また、ロボット制御装置１００およびロボット制御装置１００の外部のうちの少なくともいずれかには、モータ１０７の回転を速やかにかつ直接的に停止するための手段として、非常停止指示部１１２が設けられている。この非常停止指示部１１２が働いたとき、速やかにブレーキ１０９は保持される。すなわち、モータ１０７に対してブレーキ１０９がかかった状態となる。また、非常停止指示部１１２が働いたことにより、移動指令生成部１０１からの移動指令の送出は停止される。
【０００９】
具体的には、教示操作時に用いる教示装置に設けられたデッドマンスイッチ１１０や非常停止スイッチ１１１が、ブレーキ１０９の通電回路に連動している。これにより、デッドマンスイッチ１１０あるいは非常停止スイッチ１１１の回路が開くと、電源１１３からブレーキ１０９への通電が遮断され、モータ１０７は、ブレーキ１０９がかかった状態に保持される。なお、非常停止スイッチ１１１の例としては、教示装置に設けられたスイッチや、ロボット制御装置１００に設けられたスイッチや、マニピュレータの周りに置かれた柵の開閉を検知するリミットスッチ等が挙げられる。また、移動指令生成部１０１は、例えば、ブレーキ１０９に印加される電圧を監視することにより、非常停止指示部１１２が動作したことを検知して、移動指令の送出を即時に停止する。以下、このような一連の動作を非常停止と呼ぶことにする。
【００１０】
ここで、マニピュレータを駆動するモータ１０７の回転中に、非常停止が行われた場合、直ちにモータ１０７を制御するサーボ制御を停止すると、モータ１０７が停止するまでの回転量が大きくなる場合がある。すなわち、マニピュレータの惰走距離が大きくなる場合がある。これは、ブレーキ１０９への通電が遮断されてからブレーキ１０９が利き始めるまでのタイムラグや、ブレーキトルクだけでは十分な制動トルクが得られないといったことに起因する。
【００１１】
そこで、非常停止が行われた場合、その後もしばらくの間サーボ制御を継続して行う。これにより、モータ１０７に減速トルクを発生させ、併せて、ブレーキ１０９も用いてモータ１０７を停止させる方法が提案されている（例えば、特許文献１参照）。
【００１２】
さらには、非常停止時に、例えば、モータ１０７とマニピュレータのアームとを接続する減速機にかかる衝撃を緩和するため、もしくは、モータ１０７と減速機の接続部にかかる衝撃を緩和するため、ブレーキ１０９を、条件に応じて随時、保持したり解除したりするという方法も提案されている（例えば、特許文献２参照）。
【００１３】
なお、非常停止時に、サーボ制御を継続してモータ１０７の減速トルクを発生させるための方法として、直接的に減速トルクを発生させる以外に、速度指令をゼロへと誘導するものがある。
【００１４】
例えば、偏差カウンタ部１１５内に残存する位置偏差量をゼロクリアして速度指令を直接的にゼロにするもの、あるいは、偏差カウンタ部１１５内に残存する位置偏差量を消化しながら速度指令を漸近的にゼロにしていくもの等である。
【００１５】
図１４、図１５は、従来のロボット制御装置における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【００１６】
図１４は、非常停止時に、図１３に示す偏差カウンタ部１１５内に残存する位置偏差量を消化しながら速度指令をゼロへと誘導する場合の例を示している。すなわち、図１４は、フィードフォワード制御を行う場合の、移動指令と、位置偏差量と、速度指令と、速度指令の微分値の時間的な変化の挙動を示したものである。
【００１７】
非常停止後は、図１３の移動指令生成部１０１からの移動指令の送出が停止し、フィードフォワード項がゼロになる。従って、図１４に示すように、非常停止直後の速度指令は、不連続に変化している。なお、この速度指令が不連続に変化している部分におけるその時間微分値（速度指令の加速度）は、理屈上、無限大である。
【００１８】
また、図１５は、非常停止時に、図１３に示す偏差カウンタ部１１５内に残存する位置偏差量を消化しないで速度指令をゼロへと誘導する場合の例を示している。すなわち、図１５は、フィードフォワード制御を行わない場合の、移動指令と、位置偏差量と、速度指令およびその速度指令の微分値の時間的な変化の挙動を示したものである。
【００１９】
フィードフォワード制御を行わない場合、非常停止直後の速度指令は、図１５に示すように、不連続になることはない。しかし、図１５に示すように、速度指令の微分値は、非常停止直後に瞬間的に大きな値となる。
【００２０】
なお、図１４、図１５ともに、速度制御系と電流制御系の応答性は、位置制御系の応答性に比べて十分に高いものとし、位置制御系を時定数が１／Ｋｐである１次遅れ系とした場合を示している。
【００２１】
ここで、モータ出力トルクとブレーキトルクを合わせたトルクを総合出力トルクと呼ぶことにすると、非常停止時の減速は、総合出力トルクでなされるということになる。このとき、モータ１０７によって駆動される被駆動部の慣性が、モータ１０７の慣性に比べて十分大きい場合、総合出力トルクの大部分が、モータ１０７と被駆動部とを接続する機構部（例えば、減速機等）にかかる。
【００２２】
この総合出力トルクが、機構部の設計強度よりも小さければ、問題はない。しかし、そうでない場合、機構部にダメージを与えることになる。
【００２３】
機構部にダメージを与え始める大きさのモータ総合出力トルクを許容最大総合出力トルクと呼ぶことにする。そして、この許容最大総合出力トルクで減速したときの加速度を、限界加速度と呼ぶことにする。速度指令の時間微分値が限界加速度を超えている場合、結果として、機構部に許容最大総合トルクを超えるトルクがかかることになる。これは、非常停止による減速においても、速度制御により、モータ１０７の速度が速度指令通りとなるように制御されるからである。
【００２４】
上述したように、非常停止時に、機構部にダメージを与えないようにするためには、減速時の速度指令の微分値を限界加速度より小さく保つ必要がある。しかるに、上述した従来の方法では、そのようなことは考慮されておらず、非常停止時の速度指令の微分値は、必ずしも限界加速度より小さいとは限らない。
【００２５】
例えば、図１４に示されるような場合、非常停止直後の速度指令の微分値が限界加速度を超えることは明らかである。また、図１５に示されるような場合でも、非常停止直後の速度指令の微分値が限界加速度よりも小さいという保証はない。
【００２６】
すなわち、非常停止時の速度指令の減速のさせ方において、従来の方法では、機構部にダメージを与える可能性があるという課題があった。
【先行技術文献】
【特許文献】
【００２７】
【特許文献１】特開平４−１５２０９１号公報
【特許文献２】特開２００７−１０４８６９号公報
【発明の概要】
【００２８】
本発明は、上記問題を解決するために、機構部にダメージを与えないように非常停止を行うモータ制御装置を提供する。
【００２９】
上記課題を解決するために、本発明のモータ制御装置は、機械の可動部の相対移動を行いブレーキにより停止されるモータの制御を行うモータ制御装置であって、移動指令生成部と、サーボ制御部とを備えている。ここで、移動指令生成部は、モータの移動指令を出力する。サーボ制御部は、上記移動指令生成部の出力に基づいて上記モータを制御するための指令を出力する。上記サーボ制御部は、位置制御部と、速度制御部と、電流制御部と、処理部とを備えている。ここで、位置制御部は、上記移動指令生成部の出力に基づいて速度指令を出力する。速度制御部は、上記位置制御部の出力に基づいて電流指令を出力する。処理部は、上記モータの回転位置を検出する位置検出器の出力に基づいて上記モータの回転角の変化量を出力する。上記位置制御部は、偏差カウンタ部と、第１の係数部と、第２の係数部と、加算部と、偏差補正部と、を備えている。ここで、偏差カウンタ部は、上記移動指令生成部の出力と上記処理部の出力との偏差を求めて出力する。第１の係数部は、上記移動指令生成部の出力を入力して所定の係数を乗じて出力する。第２の係数部は、上記偏差カウンタ部の出力を入力して所定の係数を乗じて出力する。加算部は、上記第１の係数部の出力と上記第２の係数部の出力を加算して上記速度制御部に出力する。上記モータ制御装置および上記モータ制御装置の外部のうちの少なくともいずれかに設けられた非常停止指示部により上記モータの非常停止が指示されると、偏差補正部は、非常停止直前の速度指令である非常停止直前の位置制御周期で上記位置制御部が出力した速度指令値と上記第２の係数部の所定の係数とに基づいて偏差補正値を求める。一方、上記非常停止指示部により非常停止が指示されると、偏差補正部は、上記移動指令生成部による移動指令の出力を停止し、上記偏差カウンタ部の偏差を上記偏差補正部で求めた偏差補正値に置き換えて上記位置制御部による制御を行う構成からなる。
【００３０】
この構成により、非常停止時に、連続性を保ちつつ指定した割合で速度指令を低下させることができ、機構部の強度に合わせた非常停止時の速度指令の減速を行うことができる。これにより、非常停止時の機構部へのダメージを回避することができる。
【図面の簡単な説明】
【００３１】
【図１】図１は、本発明の実施の形態１におけるロボットシステムの概略構成を示す図である。
【図２】図２は、本発明の実施の形態１における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図３】図３は、本発明の実施の形態２におけるロボットシステムの概略構成を示す図である。
【図４】図４は、本発明の実施の形態２における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図５】図５は、本発明の実施の形態３におけるロボットシステムの概略構成を示す図である。
【図６】図６は、本発明の実施の形態３におけるサーボ制御部の処理フローを示す図である。
【図７】図７は、本発明の実施の形態３における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図８】図８は、本発明の実施の形態４におけるロボットシステムの概略構成を示す図である。
【図９】図９は、本発明の実施の形態４におけるサーボ制御部の処理フローを示す図である。
【図１０】図１０は、本発明の実施の形態５におけるロボットシステムの概略構成を示す図である。
【図１１】図１１は、本発明の実施の形態５におけるサーボ制御部の処理フローを示す図である。
【図１２】図１２は、本発明の実施の形態５における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図１３】図１３は、従来のロボットシステムの概略構成を示す図である。
【図１４】図１４は、従来のロボット制御装置における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【図１５】図１５は、従来のロボット制御装置における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【発明を実施するための形態】
【００３２】
以下、本発明の一実施の形態について、図面を参照しながら説明する。以下の図面においては、同じ構成要素については同じ符号を付しているので説明を省略する場合がある。
【００３３】
（実施の形態１）
本実施の形態１のモータ制御装置について、図１と図２を用いて説明する。なお、背景技術で説明した図１３と同様の箇所については、詳細な説明は省略する。なお、図１において、図１３と異なる主な点は、位置制御部３内に偏差補正部１８と判定部１９を設けた点である。
【００３４】
図１は、本発明の実施の形態１におけるロボットシステムの概略構成を示す図である。図２は、本発明の実施の形態１における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。なお、ロボットシステムは、モータ７等により駆動されるマニピュレータと、マニピュレータを制御するためのロボット制御装置と、モータ７を停止させるための非常停止指示部１２等から構成される。
【００３５】
また、マニピュレータは、モータ７により駆動されるものであり、モータ７には、位置検出器８やブレーキ９が設けられている。また、モータ制御装置であるロボット制御装置は、移動指令生成部１や、サーボ制御部２や、アンプ６を備えている。また、非常停止指示部１２は、ロボット制御装置に設けても良いし、ロボット制御装置の外部に設けても良いし、ロボット制御装置とロボット制御装置の外部の両方に設けても良い。
【００３６】
移動指令生成部１は、ユーザが作成する動作プログラムに基づいて軌道計画をし、所定の時間毎のモータ７の回転角の変化量を移動指令として生成し、サーボ制御部２へ渡す。
【００３７】
サーボ制御部２は、位置制御部３と、速度制御部４と、電流制御部５と、処理部１４とを備えている。位置制御部３および処理部１４は、位置制御周期毎に制御演算処理を行う。速度制御部４と電流制御部５は、それぞれ所定の周期毎に制御演算処理を行う。
【００３８】
位置制御部３は、移動指令生成部１からの移動指令を受けて位置制御を行い、速度指令を生成する。速度制御部４は、位置制御部３からの速度指令を受けて速度制御を行い、電流指令を生成する。電流制御部５は、速度制御部４からの電流指令を受けて電流制御を行い、電圧指令を生成する。アンプ６は、電流制御部５からの電圧指令を受けてモータ７へ与える電流を生成する。
【００３９】
モータ７には、位置検出器８が接続されており、モータ７の回転角が検出されるようになっている。処理部１４は、位置検出器８から得たモータ７の回転角の位置制御周期当りの変化量を算出し、これを実位置の変化量として出力するものである。
【００４０】
位置制御部３内の偏差カウンタ部１５は、位置制御周期毎に、移動指令生成部１からの移動指令を偏差カウンタ部１５が保持している値に加算し、一方で、処理部１４からの実位置の変化量を偏差カウンタ部１５が保持している値から減算する。そして、このように処理した偏差カウンタ部１５の出力に第２の係数部１６の位置ゲインの値Ｋｐを乗じたものと、移動指令生成部１からの移動指令に第１の係数部１７のフィードフォワード係数の値Ｋｆを乗じたものとを加算部２３で加算し、この和を速度指令として出力する。
【００４１】
また、モータ７には、モータ７を停止する機能を有するブレーキ９が接続されている。このブレーキ９は、ブレーキ９に通電することでブレーキが解除されるようになっている。なお、ブレーキ９に通電しない場合には、ブレーキがかかった状態となる。
【００４２】
電源１３からブレーキ９への通電は、非常停止指示部１２を構成するデッドマンスイッチ１０および非常停止スイッチ１１を介して行われるようになっている。すなわち、デッドマンスイッチ１０もしくは非常停止スイッチ１１の回路が開いた状態になると、電源１３からブレーキ９への通電が遮断され、ブレーキが保持された状態、すなわち、ブレーキがかかった状態となる。
【００４３】
また、ブレーキ９への通電が遮断されたことは、移動指令生成部１で検知されるようになっている。なお、移動指令生成部１は、例えば、ブレーキ９に印加される電圧を監視することで、ブレーキ９への通電が遮断されたことを検知する。移動指令生成部１は、ブレーキ９への通電が遮断されたことを検知した場合、即座に移動指令の送出を停止する。
【００４４】
なお、以下の説明において、非常停止とは、デッドマンスイッチ１０もしくは非常停止スイッチ１１の回路が開いてブレーキ９が保持され、移動指令生成部１からの移動指令の送出が停止されることを指すものとする。
【００４５】
ここで、サーボ制御部２は、非常停止されたことを次のようにして検知する。サーボ制御部２内には、判定部１９が設けられている。この判定部１９は、移動指令生成部１から位置制御部３に入力される移動指令を監視し、位置制御周期毎に定期的に送られてくる移動指令が中断された場合、非常停止されたと判断する。判定部１９は、適宜、サーボ制御部２内の非常停止情報を必要とする偏差補正部１８に、非常停止されたことを通知する。なお、移動指令生成部１からサーボ制御部２内の判定部１９へ、直接的に非常停止したことを通知して非常停止されたことを判断するようにしてもよい。
【００４６】
偏差補正部１８は、判定部１９から非常停止が行われたことを検知した旨の信号を受け取った場合、非常停止直前の速度指令値Ｖａ（前回の位置制御周期で位置制御部３が出力した速度指令値）と第２の係数部１６の位置ゲインの値Ｋｐとを用いて、下記（数１）により置換量Δθｃを求める。なお、置換量Δθｃは、後述するように、非常停止の前後で速度指令が不連続とならず連続となるようにするために用いられる値である。
【００４７】
【数１】
2013005388.tif 000031
【００４８】
そして、偏差カウンタ部１５の値を、この置換量Δθｃで置き換える。
【００４９】
これらの操作は、非常停止が検知されて以後の最初の位置制御周期で１回だけ行う。そして、それ以後もサーボ制御処理は継続して行う。
【００５０】
すなわち、偏差カウンタ部１５の偏差の偏差補正部１８で求めた偏差補正値への置き換えは、非常停止が行われた直後の最初の位置制御周期で１回だけ行い、それ以後は、偏差カウンタ部１５の値と処理部１４の出力との偏差に基づいて制御を行う構成としてもよい。この構成により、移動指令生成部１からの移動指令が停止された状態でも、偏差カウンタ部１５の値と処理部１４の出力に基づいてサーボ制御処理が継続して行われる。これにより、偏差カウンタ部１５の値が消化されてゼロとなり、すなわち、速度指令がゼロとなり、モータ７が完全に停止することとなる。
【００５１】
次に、図２に、図１の構成を用いたモータ制御装置の非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す。図２は、それぞれ、非常停止前後における、移動指令、位置偏差量Δθ（ｔ）、速度指令Ｖｃｍｄ（ｔ）、速度指令の微分値Ｖｃｍｄ’（ｔ）の時間的な変化を示している。なお、速度指令の微分値Ｖｃｍｄ’（ｔ）では、図を見やすくするため、符号を反転して記載している。図２の移動指令の時間的な変化は、非常停止したことにより移動指令の送出が停止したことを示している。なお、図２に示すように、位置偏差量Δθ（ｔ）は、偏差カウンタ部１５に保持される偏差カウンタ部値のことであり、Δθｒは非常停止直前の偏差カウンタ部の値である。また、図２の速度指令の時間的な変化において、点線で表している直線および曲線は、参考として示す従来の方法の場合の速度指令の挙動を示しており、Ｋｆ・Ｗａはフィードフォワード項に当たる。
【００５２】
図２の速度指令Ｖｃｍｄ（ｔ）の時間的な変化に関し、従来の場合は、点線の曲線で示すように、移動指令の送出が停止した直後に不連続に変化するのに対して、偏差カウンタ値を（数１）で算出した置換量Δθｃに置き換える。これにより、以下で説明するように、非常停止の直前と直後で速度指令を同じ値とすることができ、速度指令が連続的につながるようになる。
【００５３】
以下、詳細について説明する。
【００５４】
速度制御部４によって制御される速度制御系および電流制御部５によって制御される電流制御系の応答が、一般的に知られているように、位置制御部３によって制御される位置制御系の応答に比べて十分に高いものであるとする。そうすると、位置制御系は、１／Ｋｐを時定数とする１次遅れ系と見なすことができる。よって、移動指令停止時点での時間をｔ＝０として、位置偏差量の時間関数をΔθ（ｔ）とすれば、移動指令停止以後の位置偏差量Δθ（ｔ）は、下記（数２）のようになる。
【００５５】
【数２】
2013005388.tif 000032
【００５６】
また、移動指令停止後の速度指令Ｖｃｍｄ（ｔ）は、フィードフォワード項が０であるので、（数３）のようになる。
【００５７】
【数３】
2013005388.tif 000033
【００５８】
ｔ＝０における速度指令Ｖｃｍｄ（０）は、上記（数３）、上記（数１）より、
【００５９】
【数４】
2013005388.tif 000034
【００６０】
（数３）はｔ≧０において連続であること、および（数４）より、ｔ＝０において速度指令Ｖｃｍｄ（ｔ）は連続である。
【００６１】
また、速度指令Ｖｃｍｄ（ｔ）の時間微分値Ｖｃｍｄ’（ｔ）は、下記（数５）のようになる。
【００６２】
【数５】
2013005388.tif 000035
【００６３】
（数５）より、非常停止直後のＶｃｍｄ’（ｔ）の絶対値は、ｔ＝０において最大の値をとり（数６）となる。
【００６４】
【数６】
2013005388.tif 000036
【００６５】
上記（数６）で算出される値が限界加速度を超えていなければ、モータの総合出力トルクは、許容最大総合出力トルク以下となり、機構部にダメージを与えることはない。
【００６６】
なお、総合出力トルクとは、モータ出力トルクとブレーキトルクを合わせたトルクのことであり、モータ７を停止するためのトルクのことである。また、許容最大総合出力トルクとは、機構部にダメージを与え始める大きさのモータ総合トルクのことである。また、限界加速度とは、この許容最大総合出力トルクで減速したときの加速度のことである。
【００６７】
以上のように、非常停止時に偏差カウンタ部１５の値を置き換えることにより、非常停止時にも、速度指令を不連続とせず、連続とすることができる。
【００６８】
これにより、非常停止時に機構部へ過大なトルクがかかるのを防止することができ、非常停止時の機構部へのダメージを回避することができる。
【００６９】
すなわち、本発明のモータ制御装置は、機械の可動部の相対移動を行いブレーキ９により停止されるモータ７の制御を行うモータ制御装置であって、移動指令生成部１と、サーボ制御部２とを備えている。ここで、移動指令生成部１は、モータ７の移動指令を出力する。サーボ制御部２は、移動指令生成部１の出力に基づいてモータ７を制御するための指令を出力する。サーボ制御部２は、位置制御部３と、速度制御部４と、電流制御部５と、処理部１４とを備えている。ここで、位置制御部３は、移動指令生成部１の出力に基づいて速度指令を出力する。速度制御部４は、位置制御部３の出力に基づいて電流指令を出力する。処理部１４は、モータ７の回転位置を検出する位置検出器８の出力に基づいてモータ７の回転角の変化量を出力する。位置制御部３は、偏差カウンタ部１５と、第１の係数部１７と、第２の係数部１６と、加算部２３と、偏差補正部１８と、を備えている。ここで、偏差カウンタ部１５は、移動指令生成部１の出力と処理部１４の出力との偏差を求めて出力する。第１の係数部１７は、移動指令生成部１の出力を入力して所定の係数を乗じて出力する。第２の係数部１６は、偏差カウンタ部１５の出力を入力して所定の係数を乗じて出力する。加算部２３は、第１の係数部１７の出力と第２の係数部１６の出力を加算して速度制御部４に出力する。モータ制御装置およびこのモータ制御装置の外部のうちの少なくともいずれかに設けられた非常停止指示部１２によりモータ７の非常停止が指示されると、偏差補正部１８は、非常停止直前の速度指令である非常停止直前の位置制御周期で位置制御部３が出力した速度指令値と第２の係数部１６の所定の係数とに基づいて偏差補正値を求める。一方、非常停止指示部１２により非常停止が指示されると、偏差補正部１８は、移動指令生成部１による移動指令の出力を停止し、偏差カウンタ部１５の偏差を偏差補正部１８で求めた偏差補正値に置き換えて位置制御部３による制御を行う構成からなる。
【００７０】
この構成により、非常停止時に、連続性を保ちつつ指定した割合で速度指令を低下させることができ、機構部の強度に合わせた非常停止時の速度指令の減速を行うことができる。これにより、非常停止時の機構部へのダメージを回避することができる。
【００７１】
（実施の形態２）
本実施の形態２のモータ制御装置について、図３と図４を用いて説明する。図３は、本発明の実施の形態２におけるロボットシステムの概略構成を示す図である。図４は、本発明の実施の形態２における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【００７２】
実施の形態１で示したモータ制御装置の構成により、非常停止時も、速度指令が不連続になることを防止し、速度指令の微分値を下げることができる。しかし、例えば、第２の係数部１６の位置ゲインの値Ｋｐが大きい場合、限界加速度を超える場合がある。そこで、これに対応するための別の例として、実施の形態２のモータ制御装置について説明する。
【００７３】
本実施の形態２のロボットシステムの概略構成が実施の形態１と異なる主な点は、実施の形態１で説明した図１の構成に、係数補正部２２を加えた点である。
【００７４】
係数補正部２２は、後述のように値Ｋｐｃを求め、第２の係数部１６の位置ゲインの値Ｋｐを値Ｋｐｃに置き換える処理を行うものである。
【００７５】
判定部１９により非常停止であることが検知された場合、係数補正部２２は、値Ｋｐｃを次のようにして求める。すなわち、限界加速度の値をＡｃ（＞０）とし、非常停止の直前の位置制御部２１が出力する速度指令値をＶａとして、下記（数７）で演算して求める。
【００７６】
【数７】
2013005388.tif 000037
【００７７】
そして、第２の係数部１６の位置ゲインの値をこのＫｐｃに置き換える。
【００７８】
すなわち、本実施の形態２のモータ制御装置は、係数の補正値を求める係数補正部２２を備え、非常停止指示部１２により非常停止が指示されると、第２の係数部１６の所定の係数を係数補正部２２で求めた係数補正値に置き換えて位置制御部２１による制御を行う構成としてもよい。ここで、係数補正部２２は、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、非常停止が指示される直前の位置制御部２１が出力する速度指令値とに基づいて、係数の補正値を求めている。この構成により、非常停止時に、連続性を保ちつつ指定した割合で速度指令を低下させることができ、機構部の強度に合わせた非常停止時の速度指令の減速を行うことができる。これにより、非常停止時の機構部へのダメージを回避することができる。
【００７９】
次に、偏差補正部１８において、この位置ゲインの値Ｋｐｃと速度指令値Ｖａとから、置換量Δθｃを下記（数８）により求める。
【００８０】
【数８】
2013005388.tif 000038
【００８１】
そして、偏差カウンタ部１５の値をこの置換量Δθｃに置き換える。
【００８２】
なお、係数補正部２２および偏差補正部１８によるこれらの操作は、非常停止が検知されて以後の最初の位置制御周期で１回だけ行う。そして、サーボ制御部２０は、それ以後も置き換えた係数補正値を用いて制御を行う。これにより、サーボ制御処理は継続して行われる。
【００８３】
すなわち、本実施の形態２のモータ制御装置において、第２の係数部１６の所定の係数の係数補正部２２で求めた係数補正値への置き換えは、非常停止が行われた直後の最初の位置制御周期で１回だけ行い、それ以後は、置き換えた係数補正値を用いて制御を行う構成としてもよい。この構成により、移動指令生成部１からの移動指令が停止された状態でも、偏差カウンタ部１５の値と処理部１４の出力に基づいてサーボ制御処理が継続して行われる。これにより、偏差カウンタ部１５の値が消化されてゼロとなり、すなわち、速度指令がゼロとなり、モータ７が完全に停止することとなる。
【００８４】
係数補正部２２および偏差補正部１８により、これらの操作を行ったときの、非常停止直後の速度指令の微分値は、上記（数６）と（数７）より、
｜Ｖｃｍｄ’（０）｜＝｜Ｋｐｃ・Ｖａ｜＝Ａｃ
となる。
【００８５】
なお、限界加速度Ａｃの値は、機構部の強度に合わせて予め決めておいた値とする。
【００８６】
次に、図４に、図３のモータ制御装置の構成を用いたときの、非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す。図４の時間的な変化は、上からそれぞれ、非常停止前後における、移動指令、位置偏差量Δθ（ｔ）、速度指令Ｖｃｍｄ（ｔ）および速度指令の微分値Ｖｃｍｄ’（ｔ）を示している。図４は、図２で示したものと同じ物理量のものを示す。
【００８７】
図４には、図３に示す係数補正部２２により位置ゲインの値がＫｐからＫｐｃへと変更されても、速度指令Ｖｃｍｄ（ｔ）は、連続的につながることが示されている。
【００８８】
また、図４には、位置ゲインの値を上記（数７）のようにして決めることで、速度指令の微分値は、限界加速度以下に抑えられることが示されている。
【００８９】
以上のように、非常停止時に、偏差カウンタ部１５の値および第２の係数部１６の位置ゲインの値を置き換えることにより、連続性を保ちつつ指定した割合で速度指令を低下させることができ、限界加速度以下とすることができる。これにより、機構部の強度に合わせた、非常停止時の速度指令減速を行うことができ、非常停止時の機構部へのダメージを回避することができる。
【００９０】
（実施の形態３）
本実施の形態３のモータ制御装置について、図５から図７を用いて説明する。図５は、本発明の実施の形態３におけるロボットシステムの概略構成を示す図である。図６は、発明の実施の形態３におけるサーボ制御部３０の処理フローを示す図である。図７は、本発明の実施の形態３における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【００９１】
本実施の形態３のモータ制御装置が実施の形態１と異なる主な点は、実施の形態１で説明した図１の構成に、偏差更新部３２を加えた点である。なお、偏差更新部３２は、位置制御周期毎に偏差カウンタ部１５の値を補正する部分である。
【００９２】
判定部１９により非常停止が検知された場合、まず、偏差補正部１８で、非常停止直前の速度指令値Ｖａ（前回の位置制御周期で位置制御部３１が出力した速度指令値）と第２の係数部１６の位置ゲインの値Ｋｐとを用いて、実施の形態１で示した（数１）により置換量Δθｃを求める。そして、偏差カウンタ部１５の値をこの置換量Δθｃに置き換える。なお、これらの操作は、非常停止後に１回だけ行う。
【００９３】
次に、詳細は後述するが、偏差更新部３２により、偏差カウンタ部１５の値を位置制御周期毎に補正していく。なお、偏差更新部３２の処理は、非常停止が検知されて以後に行う。
【００９４】
偏差更新部３２による補正を含む詳細な処理について、図６を用いて説明する。
【００９５】
なお、図６に示すステップＳ１とステップＳ２は、非常停止を検知して以後に一度だけ行い、ステップＳ３を含むそれ以後のステップは、位置制御周期毎に行う。
【００９６】
ステップＳ１は、上述の偏差補正部１８における処理である。
【００９７】
ステップＳ１の処理を行った後、ステップＳ２において、非常停止直前の速度指令値をＶａ、限界加速度をＡｃ、位置制御周期をＴｐとして、下記（数９）により、位置偏差量Δθ（ｔ）の減少の割合を一定にするための制限量Ｄを求める。
【００９８】
【数９】
2013005388.tif 000039
【００９９】
ここで、制限量Ｄは、符号付きの量である。
【０１００】
ステップ３において、非常停止を指示してから所定時間が経過していなければ、ステップＳ４、さらにステップＳ５へと進む。なお、非常停止を指示してから所定時間が経過した場合については後述する。
【０１０１】
ステップＳ４およびステップＳ５は、実施の形態１において図１を用いて説明した位置制御演算に相当する。すなわち、ステップＳ４では、位置制御周期当りの実位置変化量を求め、ステップＳ５では、偏差カウンタ部１５から実位置変化量を差し引いて偏差カウンタ部１５の値を更新する。
【０１０２】
なお、以下では、実位置変化量を差し引く前の偏差カウンタ部１５の位置偏差カウント値を、「前回偏差」といい、実位置変化量を差し引いた後の位置偏差カウンタ値を「今回偏差」ということにする。
【０１０３】
ステップＳ７において、ステップＳ２で求めた制限量Ｄを前回偏差から差し引いた値の絶対値と、ステップＳ５で求めた今回偏差の絶対値との比較をする。そして、前者の方が大きければ、ステップＳ８において、今回偏差を、前回偏差から制限量Ｄを差し引いた値で置き換える。
【０１０４】
ステップＳ１１において、上記のように補正した今回偏差に、位置ゲインの値Ｋｐを乗じて、速度指令とする。
【０１０５】
さらに、この速度指令に基づいて、ステップＳ１２として、速度制御部４による速度制御と、電流制御部５による電流制御を行う。なお、速度制御と電流制御は、位置制御周期当りに１回とは限らず、複数回行われることもある。
【０１０６】
また、ステップＳ１３のように、今回偏差は、次の位置制御周期における前回偏差となる。
【０１０７】
なお、ステップＳ３に示すように、非常停止から所定時間が経過した場合は、サーボ制御を停止するようにしている。しかし、サーボ制御を停止せず、継続するようにしてもかまわない。その場合は、偏差カウンタ部が０になるように位置制御がなされる。
【０１０８】
次に、図７に、図５のモータ制御装置の構成を用いたときの、非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す。図７の時間的な変化は、上からそれぞれ、非常停止前後における、移動指令、位置偏差量Δθ（ｔ）、速度指令Ｖｃｍｄ（ｔ）および速度指令の微分値Ｖｃｍｄ’（ｔ）を示している。図７は、図２で示したものと同じ物理量のものを示す。
【０１０９】
図７に示す位置偏差量Δθ（ｔ）は、実速度（実位置の変化量）の大きさに応じた割合で減少していこうとする。しかし、ステップＳ５からステップＳ１１までの処理により、位置偏差量Δθ（ｔ）の減少の割合には制限がかかる。結果として、位置偏差量Δθ（ｔ）は、１次関数の形で減少していく。よって、図７に示す速度指令Ｖｃｍｄ（ｔ）も１次関数の形で減少していき、その微分値は一定値となる。
【０１１０】
具体的には、非常停止後の位置偏差量Δθ（ｔ）と、速度指令値Ｖｃｍｄ（ｔ）と、速度指令の微分値Ｖｃｍｄ’（ｔ）は、それぞれ、下記（数１０）、（数１１）、（数１２）のようになる。
【０１１１】
【数１０】
2013005388.tif 000040
【０１１２】
【数１１】
2013005388.tif 000041
【０１１３】
【数１２】
2013005388.tif 000042
【０１１４】
さらに、（数１２）と（数１）、（数９）より、下記（数１３）を得る。
【０１１５】
【数１３】
2013005388.tif 000043
【０１１６】
なお、速度指令が低下していって、｜Ｔｐ・Ｖｃｍｄ（ｔ）｜≦｜制限量Ｄ｜となる区間（ｔ≧ｔ１）では、位置偏差量Δθ（ｔ）は、その減少に制限がかからなくなり、図２または図４で示したものと同じ指数関数の形となる。よって、速度指令Ｖｃｍｄ（ｔ）も同様の形となり、また、その微分値も限界加速度Ａｃよりも小さい値となる。
【０１１７】
すなわち、本実施の形態３のモータ制御装置は、非常停止指示部１２により非常停止が指示されて偏差カウンタ部１５の偏差を偏差補正部１８で求めた偏差補正値に置き換えた後に、偏差カウンタ部１５の値を位置制御周期毎に補正する偏差更新部３２を備えている。そして、偏差更新部３２は、非常停止直前の速度指令値と、限界加速度と、位置制御周期と、偏差補正値と、前回偏差と、に基づいて、今回偏差を求め、位置制御周期毎に偏差カウンタ部１５の偏差を求めた今回偏差に置き換えて制御を行う構成としてもよい。ここで、限界加速度は、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である。偏差補正値は、偏差補正部１８で求められる。前回偏差は、処理部１４の出力である実位置変化量を差し引く前の偏差カウンタ部１５の値である。今回偏差は、位置制御周期毎に実位置変化量を差し引いた後の偏差カウンタ部１５の値として求められる。
【０１１８】
この構成により、非常停止時に偏差カウンタ部１５の値を置き換え、さらに、偏差カウンタ部１５の値を補正しながら位置制御を行うことで、速度指令を、連続性を保ちつつ一定の割合で低下させることができる。これにより、非常停止時の機構部へのダメージを回避しながら、不必要に減速距離を伸ばすことなく減速停止させることができる。
【０１１９】
（実施の形態４）
本実施の形態４のモータ制御装置について、図８と図９を用いて説明する。図８は、本発明の実施の形態４におけるロボットシステムの概略構成を示す図である。図９は、本発明の実施の形態４におけるサーボ制御部４０の処理フローを示す図である。
【０１２０】
本実施の形態４のロボットシステムの概略構成が実施の形態１と異なる主な点は、実施の形態１で説明した図１の構成に、第１の位置補正部４２を加えた点である。なお、第１の位置補正部４２は、位置制御周期毎に偏差カウンタ部１５から差し引く実位置変化量を補正する部分である。
【０１２１】
判定部１９により非常停止が検知されたら、まず偏差補正部１８で、非常停止直前の速度指令値Ｖａ（前回の位置制御周期で位置制御部４１が出力した速度指令値）と第２の係数部１６の位置ゲインの値Ｋｐとを用いて、実施の形態１で説明した（数１）により置換量Δθｃを求める。そして、偏差カウンタ部１５の値を、この置換量Δθｃで置き換える。なお、これらの操作は非常停止後に１回だけ行う。
【０１２２】
次に、詳細は後述するが、第１の位置補正部４２において、偏差カウンタ部１５から差し引く実位置変化量の値を位置制御周期毎に補正していく。なお、第１の位置補正部４２の処理は、非常停止が検知されて以後に行う。
【０１２３】
第１の位置補正部４２による補正を含む詳細な処理について、図９を用いて説明する。なお、図９に示すように、実施の形態３で用いた図６と同じ処理を行うステップについては、図６と同じ符号を付している。また、図９に示すステップＳ１とステップＳ２は、非常停止を検知して以後に一度だけ行い、ステップＳ３を含むそれ以後のステップは、位置制御周期毎に行う。
【０１２４】
ステップＳ１は、上述の偏差補正部１８における処理である。ステップＳ１の処理を行った後、ステップＳ２において、非常停止直前の速度指令値Ｖａと、限界加速度Ａｃと、位置制御周期Ｔｐとを用いて、実施の形態３で示した（数９）により制限量Ｄを求める。ここで、制限量Ｄは、符号付きの量である。
【０１２５】
ステップＳ３において、非常停止してから所定時間が経過していなければ、ステップＳ４以降へと進む。ステップＳ４において、位置制御周期当りの実位置変化量を求める。この後は、図９に示すようにステップＳ２０に進む。なお、非常停止してから所定時間が経過した場合については後述する。
【０１２６】
次に、ステップＳ２０において、ステップＳ４で求めた実位置変化量を位置フィードバック量とする。なお、位置フィードバック量は、図９のステップＳ２０では、位置ＦＢ量と記載している。
【０１２７】
ステップＳ２１において、位置フィードバック量の絶対値と制限量Ｄの絶対値との比較を行い、位置フィードバック量の絶対値の方が大きければ、ステップＳ２３において、位置フィードバック量を制限量Ｄで置き換える。
【０１２８】
なお、ステップＳ２１における判定で、位置フィードバック量の絶対値の大きさが制限量Ｄの絶対値より大きくない場合、位置フィードバック量の置き換えは行わない。
【０１２９】
ステップＳ２５において、上記のようにして求めた位置フィードバック量を、前回偏差から差し引いた値を、今回偏差とする。ここで、今回偏差、前回偏差の意味は、実施の形態３で説明したものと同じである。
【０１３０】
次に、ステップＳ１１において、今回偏差に位置ゲインの値Ｋｐを乗じて速度指令を算出する。そして、ステップＳ１２において速度制御および電流制御を行う。さらに、ステップＳ１３において、今回偏差を次回の前回偏差として記憶し、ステップＳ３へと戻る。
【０１３１】
なお、図８の構成を用いたときの非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動は、実施の形態３で説明した図７と同じになるので、説明は省略する。
【０１３２】
本実施の形態４のモータ制御装置は、位置制御周期毎に処理部１４の出力を補正して偏差カウンタ部１５に出力する第１の位置補正部４２を備えている。この第１の位置補正部４２は、処理部１４で求めた実位置変化量と、非常停止直前の速度指令値と、限界加速度と、位置制御周期と、偏差補正部１８で求めた偏差補正値に基づいて、偏差カウンタ部１５に出力する位置フィードバック量を求める構成としてもよい。ここで、限界加速度は、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である。
【０１３３】
この構成により、非常停止時に偏差カウンタ部１５の値を置き換え、さらに、偏差カウンタ部１５への位置フィードバック量を補正しながら位置制御を行う。そうすると、速度指令を、連続性を保ちつつ一定の割合で低下させることができる。これにより、非常停止時の機構部へのダメージを回避しながら、不必要に減速距離を伸ばすことなく減速停止させることができる。
【０１３４】
（実施の形態５）
本実施の形態５のモータ制御装置について、図１０から図１２を用いて説明する。図１０は、本発明の実施の形態５におけるロボットシステムの概略構成を示す図である。図１１は、本発明の実施の形態５におけるサーボ制御部５０の処理フローを示す図である。図１２は、本発明の実施の形態５における非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す図である。
【０１３５】
本実施の形態５のモータ制御装置が実施の形態１と異なる主な点は、実施の形態１で説明した図１の構成に、第２の位置補正部５２を加えた点である。なお、この第２の位置補正部５２は、図８を用いて実施の形態４で説明した第１の位置補正部４２とは異なるものである。
【０１３６】
第２の位置補正部５２は、位置制御周期毎に偏差カウンタ部１５から差し引く実位置変化量を補正する部分である。第２の位置補正部５２は、位置制御周期毎に実位置変化量を積算していったものを位置積算量として保持する。そして、第２の位置補正部５２は、速度指令がゼロになるまで、偏差カウンタ部１５の減少の割合を一定にするために、位置積算量の中から位置制御周期毎に一定量を取り出して位置フィードバックとして出力する。
【０１３７】
本実施の形態５のモータ制御装置において、非常停止指示部１２により非常停止が指示されると、実位置変化量の積算量をゼロにした後に実位置変化量の積算を行い、実位置変化量の積算量をゼロにする処理は、前記非常停止がされたことを検知して以後一度だけ行われる構成としてもよい。これにより、非常停止時の機構部へのダメージを回避しながら、必要かつ最短の減速距離で減速停止させることができる。
【０１３８】
判定部１９により非常停止が検知された場合、まず、偏差補正部１８で、非常停止直前の速度指令値Ｖａ（前回の位置制御周期で位置制御部５１が出力した速度指令値）と第２の係数部１６の位置ゲインの値Ｋｐとを用いて、実施の形態１で説明した（数１）により置換量Δθｃを求める。そして、偏差カウンタ部１５の値をこの置換量Δθｃで置き換える。なお、これらの操作は非常停止後に１回だけ行う。
【０１３９】
次に、第２の位置補正部５２で、位置制御周期毎に処理部１４から入力した実位置変化量を補正処理して位置フィードバック量を算出する。そして、位置制御周期毎に偏差カウンタ部１５からこの位置フィードバック量を差し引く。なお、第２の位置補正部５２の処理は、非常停止が検知されて以後に行う。また、第２の位置補正部５２内の位置積算量は、非常停止が検知されて以後の最初の位置制御周期に１回だけゼロクリアされる。
【０１４０】
第２の位置補正部５２による補正方法を含む詳細な処理のフローを、図１１に示す。なお、図１１において、実施の形態３で説明した図６や、実施の形態４で説明した図９と同じ処理を行うステップについては、図６や図９で用いたものと同じ符号を付している。なお、図１１におけるステップＳ１、ステップＳ２およびステップＳ３０の処理は、非常停止を検知して以後に一度だけ行う。そして、ステップＳ３を含むそれ以後のステップは、位置制御周期毎に行う。
【０１４１】
ステップＳ１は、上述の偏差補正部１８の処理である。ステップＳ１の処理を行った後、ステップＳ２において、非常停止直前の速度指令値Ｖａと、限界加速度Ａｃと、位置制御周期Ｔｐを用いて、実施の形態３で示した（数９）により制限量Ｄを求める。ここで、制限量Ｄは、符号付きの量である。
【０１４２】
図１０に示す第２の位置補正部５２には、位置積算量が含まれ、ステップＳ３０ではこれをゼロクリアする。ステップＳ３において、非常停止してから所定時間が経過していなければ、ステップＳ４以降へと進み、ステップＳ４で位置制御周期当りの実位置変化量を求める。
【０１４３】
次に、ステップＳ３１において、ステップＳ４で求めた実位置変化量を、位置積算量に加算する。ステップＳ３２において、ステップＳ３１で求めた位置積算量の絶対値とステップＳ２で求めた制限量Ｄの絶対値との比較を行う。位置積算量の絶対値の方が大きければ、ステップＳ２３において、位置フィードバック量を制限量Ｄで置き換える。
【０１４４】
ステップＳ３２における判定で、位置積算量の絶対値の大きさが制限量Ｄの絶対値より大きくない場合は、ステップＳ３４において、位置フィードバック量は位置積算量とする。
【０１４５】
このようにして位置フィードバック量（図１１では、「位置ＦＢ量」とする）を求め、ステップＳ３５において、位置積算量から位置フィードバック量を差し引いて、次回の位置積算量とする。また、ステップＳ２５において、前回偏差から位置フィードバック量を差し引いて今回偏差を求める。
【０１４６】
次に、ステップＳ１１において、今回偏差に位置ゲインの値Ｋｐを乗じて速度指令を算出する。そして、ステップＳ１２において、速度制御および電流制御を行い、ステップＳ１３において、今回偏差を次回の前回偏差として記憶し、ステップＳ３へと戻る。
【０１４７】
次に、図１２に、図１０のモータ制御装置の構成を用いたときの非常停止時の移動指令、位置偏差量、速度指令および速度指令値の微分値の時間的な変化の挙動を示す。図１２の時間的な変化は、上からそれぞれ、非常停止前後における、移動指令、位置偏差量Δθ（ｔ）、速度指令Ｖｃｍｄ（ｔ）および速度指令の微分値Ｖｃｍｄ’（ｔ）を示している。図１２は、図２で示したものと同じ物理量のものを示す。
【０１４８】
位置偏差量Δθ（ｔ）は、実速度（実位置の変化量）の大きさに応じた割合で減少していこうとする。しかし、ステップＳ３１からステップＳ３５までの処理により、位置偏差量Δθ（ｔ）の減少の割合には制限がかかる。結果として位置偏差量Δθ（ｔ）は１次関数の形で減少していく。よって、速度指令Ｖｃｍｄ（ｔ）も１次関数の形で減少していき、その微分値は一定値となる。
【０１４９】
具体的には、非常停止後の全区間にわたって、実施の形態３で示した（数１０）から（数１３）までの式で示される形となる。
【０１５０】
なお、速度が低下していって、｜Ｔｐ・Ｖｃｍｄ（ｔ）｜≦制限量Ｄとなって以後も、｜位置積算量｜＞制限量Ｄであるので、位置偏差量がゼロになる直前までは、（数１０）から（数１３）までの式で表される状態が保たれる。
【０１５１】
すなわち、位置偏差量がゼロになる直前までの全域にわたって、速度指令の微分値は一定であり、限界加速度Ａｃに保たれる。
【０１５２】
すなわち、本実施の形態５のモータ制御装置は、位置制御周期毎に処理部１４の出力を補正して偏差カウンタ部１５に出力する第２の位置補正部５２を備えている。そして、第２の位置補正部５２は、処理部１４で求めた実位置変化量を位置制御周期毎に積算した積算量と、非常停止直前の速度指令値と、限界加速度と、位置制御周期と、偏差補正部１８で求めた偏差補正値に基づいて、偏差カウンタ部１５に出力する位置フィードバック量を求める構成としてもよい。ここで、限界加速度は、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である。
【０１５３】
この構成により、非常停止時に、偏差カウンタ部１５の値を置き換え、さらに、偏差カウンタ部１５への位置フィードバック量を一定に保ちながら、位置制御を行う。そうすると、速度指令を、連続性を保ちつつ一定の割合で低下させることができる。これにより、非常停止時の機構部へのダメージを回避しながら、最短の減速距離で減速停止させることができる。
【産業上の利用可能性】
【０１５４】
本発明のモータ制御装置は、非常停止時の減速で機構部に過大なトルクがかかるのを防止することができ、機構部へのダメージを回避できるので非常停止を行うシステム等に用いるモータ制御装置として産業上有用である。
【符号の説明】
【０１５５】
１  移動指令生成部
２，２０，３０，４０，５０  サーボ制御部
３，２１，３１，４１，５１  位置制御部
４  速度制御部
５  電流制御部
６  アンプ
７  モータ
８  位置検出器
９  ブレーキ
１０  デッドマンスイッチ
１１  非常停止スイッチ
１２  非常停止指示部
１３  電源
１４  処理部
１５  偏差カウンタ部
１６  第２の係数部
１７  第１の係数部
１８  偏差補正部
１９  判定部
２２  係数補正部
２３  加算部
３２  偏差更新部
４２  第１の位置補正部
５２  第２の位置補正部
【手続補正２】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
機械の可動部の相対移動を行いブレーキにより停止されるモータの制御を行うモータ制御装置であって、
前記モータの移動指令を出力する移動指令生成部と、
前記移動指令生成部の出力に基づいて前記モータを制御するための指令を出力するサーボ制御部とを備え、
前記サーボ制御部は、
前記移動指令生成部の出力に基づいて速度指令を出力する位置制御部と、
前記位置制御部の出力に基づいて電流指令を出力する速度制御部と、
前記速度制御部の出力に基づいて電圧指令を出力する電流制御部と、
前記モータの回転位置を検出する位置検出器の出力に基づいて前記モータの回転角の変化量を出力する処理部と、を備え、
前記位置制御部は、
前記移動指令生成部の出力と前記処理部の出力との偏差を求めて出力する偏差カウンタ部と、
前記移動指令生成部の出力を入力して所定の係数を乗じて出力する第１の係数部と、
前記偏差カウンタ部の出力を入力して所定の係数を乗じて出力する第２の係数部と、
前記第１の係数部の出力と前記第２の係数部の出力を加算して前記速度制御部に出力する加算部と、
前記モータ制御装置および前記モータ制御装置の外部のうちの少なくともいずれかに設けられた非常停止指示部により前記モータの非常停止が指示されると、非常停止直前の速度指令である非常停止直前の位置制御周期で前記位置制御部が出力した速度指令値と前記第２の係数部の所定の係数とに基づいて偏差補正値を求める偏差補正部と、を備え、
前記非常停止指示部により非常停止が指示されると、前記移動指令生成部による移動指令の出力を停止し、前記偏差カウンタ部の偏差を前記偏差補正部で求めた偏差補正値に置き換えて前記位置制御部による制御を行う
モータ制御装置。
【請求項２】
前記偏差カウンタ部の偏差の偏差補正部で求めた偏差補正値への置き換えは、非常停止が行われた直後の最初の位置制御周期で１回だけ行い、それ以後は、前記偏差カウンタ部の値と前記処理部の出力との偏差に基づいて制御を行う請求項１記載のモータ制御装置。
【請求項３】
機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、非常停止が指示される直前の前記位置制御部が出力する速度指令値とに基づいて、係数の補正値を求める係数補正部を備え、
前記非常停止指示部により非常停止が指示されると、前記第２の係数部の所定の係数を前記係数補正部で求めた係数補正値に置き換えて前記位置制御部による制御を行う請求項１または２のいずれか１項に記載のモータ制御装置。
【請求項４】
前記第２の係数部の所定の係数の係数補正部で求めた係数補正値への置き換えは、非常停止が行われた直後の最初の位置制御周期で１回だけ行い、それ以後は、置き換えた前記係数補正値を用いて制御を行う請求項３記載のモータ制御装置。
【請求項５】
前記非常停止指示部により非常停止が指示されて前記偏差カウンタ部の偏差を前記偏差補正部で求めた偏差補正値に置き換えた後に、前記偏差カウンタ部の値を位置制御周期毎に補正する偏差更新部を備え、
前記偏差更新部は、非常停止直前の速度指令値と、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、位置制御周期と、前記偏差補正部で求めた偏差補正値と、前記処理部の出力である実位置変化量を差し引く前の前記偏差カウンタ部の値である前回偏差に基づいて、前記位置制御周期毎に前記実位置変化量を差し引いた後の前記偏差カウンタ部の値としての今回偏差を求め、前記位置制御周期毎に前記偏差カウンタ部の偏差を求めた前記今回偏差に置き換えて制御を行う請求項１または２のいずれか１項に記載のモータ制御装置。
【請求項６】
位置制御周期毎に前記処理部の出力を補正して前記偏差カウンタ部に出力する第１の位置補正部を備え、
前記第１の位置補正部は、前記処理部で求めた実位置変化量と、非常停止直前の速度指令値と、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、位置制御周期と、前記偏差補正部で求めた偏差補正値に基づいて、前記偏差カウンタ部に出力する位置フィードバック量を求める請求項１または２のいずれか１項に記載のモータ制御装置。
【請求項７】
位置制御周期毎に前記処理部の出力を補正して前記偏差カウンタ部に出力する第２の位置補正部を備え、
前記第２の位置補正部は、前記処理部で求めた実位置変化量を位置制御周期毎に積算した積算量と、非常停止直前の速度指令値と、機械の機構部にダメージを与え始める大きさのモータ総合トルクである許容最大総合出力トルクで減速したときの加速度である限界加速度と、位置制御周期と、前記偏差補正部で求めた偏差補正値に基づいて、前記偏差カウンタ部に出力する位置フィードバック量を求める請求項１または２のいずれか１項に記載のモータ制御装置。
【請求項８】
前記非常停止指示部により非常停止が指示されると、前記実位置変化量の積算量をゼロにした後に前記実位置変化量の積算を行い、前記実位置変化量の前記積算量をゼロにする処理は、前記非常停止がされたことを検知して以後一度だけ行われる請求項７記載のモータ制御装置。
【手続補正３】
【補正対象書類名】図面
【補正対象項目名】全図
【補正方法】変更
【補正の内容】
【図１】
2013005388.tif 000044
【図２】
2013005388.tif 000045
【図３】
2013005388.tif 000046
【図４】
2013005388.tif 000047
【図５】
2013005388.tif 000048
【図６】
2013005388.tif 000049
【図７】
2013005388.tif 000050
【図８】
2013005388.tif 000051
【図９】
2013005388.tif 000052
【図１０】
2013005388.tif 000053
【図１１】
2013005388.tif 000054
【図１２】
2013005388.tif 000055
【図１３】
2013005388.tif 000056
【図１４】
2013005388.tif 000057
【図１５】
2013005388.tif 000058
【国際調査報告】
2013005388.tif 000059
2013005388.tif 000060