(57)【要約】【課題】  移動禁止領域の境界からのオーバートラベル量が少なく、停止時にモータや機械系に大きな衝撃を与えることなく安全に停止できる数値制御装置と数値制御方法を提供する。【解決手段】  可動体７４の移動禁止位置を設定するソフトリミット設定手段５７と、設定減速度を設定する減速度設定手段５８と、第１速度指令の速度を前記設定減速度で減速させて停止させたときの設定減速停止距離が、可動禁止領域の禁止位置と現在の位置との差である移動可能距離よりも小さくなるように第１速度指令を減少させ第３速度指令を生成する移動禁止位置不可侵手段５９とを備えた。【選択図】図２
2007052590.tif 000002
【特許請求の範囲】
【請求項１】
一定の制御時間ごとにアプリケーションプログラムを解析し位置制御モード時は位置指令を、速度制御モード時は第１速度指令を生成するプログラム解析手段と、前記位置指令とモータ位置との差から第２速度指令を生成する位置制御手段と、第１速度指令と第２速度指令の和を速度指令とし、前記速度指令と速度の差からトルク指令を生成する速度制御手段と、前記トルク指令から電流指令を生成し、前記電流指令と電流の差から電圧指令を生成してモータを駆動するモータ駆動手段と、前記電流を検出する電流検出手段と、モータに結合した位置検出器の生成するモータ位置をモータ速度に変換する位置速度変換手段と、前記可動体の移動禁止位置を設定するソフトリミット設定手段とを有し、前記モータを駆動して前記可動体を移動させる数値制御装置において、
所定の減速度を設定する減速度設定手段と、
前記第１速度指令の指令速度を前記所定の減速度で減速停止させたときの減速停止距離が、前記移動禁止位置と現在のモータ位置との差である移動可能距離に等しくなるように前記第１速度指令を変更し、第３速度指令を生成する移動禁止位置不可侵手段と、
を備え、
前記第２速度指令と前記第３速度指令との和を前記速度指令とすることを特徴とする数値制御装置。
【請求項２】
前記減速停止距離は、前記モータ速度を前記所定の減速度で減速させて停止させたときの距離とすることを特徴とする請求項１記載の数値制御装置。
【請求項３】
前記移動禁止位置不可侵手段は、前記減速停止距離が前記移動可能距離よりも大きいときは前記第３速度指令から所定の微小速度を減算し新たな第３速度指令とすることを特徴とする請求項１記載の数値制御装置。
【請求項４】
前記モータ位置は、前記速度指令を積分して得ることを特徴とする請求項１記載の数値制御装置。
【請求項５】
前記モータ位置は、前記速度検出器の生成するモータ速度を積分して得ることを特徴とする請求項１記載の数値制御装置。
【請求項６】
一定の制御時間ごとにアプリケーションプログラムを解析し位置制御モード時は位置指令を生成、速度制御モード時は第１速度指令を生成するプログラム解析手段と、前記位置指令とモータ位置との差から第２速度指令を生成する位置制御手段と、第１速度指令と第２速度指令の和を速度指令とし、前記速度指令と速度の差からトルク指令を生成する速度制御手段と、前記トルク指令から電流指令を生成し、前記電流指令と電流の差から電圧指令を生成しモータを駆動するモータ駆動手段と、前記電流を検出する電流検出手段と、モータに結合した位置検出器の生成するモータ位置をモータ速度に変換する位置速度変換手段と、前記可動体の移動禁止位置を設定するソフトリミット設定手段とを有し、前記モータを駆動して可動体を移動させる数値制御方法において、
運転前に、前記可動体の移動禁止位置をソフトリミットとして設定するステップと、
所定の減速度を設定するステップと、
運転時に、制御時間ごとに、前記第１速度指令の指令速度を前記所定の減速度で減速させて停止させたときの減速停止距離を演算するステップと、
前記移動禁止位置と現在のモータ位置との差である移動可能距離を演算するステップと、
前記減速停止距離が前記移動可能距離に等しくなるように前記第１速度指令を変更し第３速度指令を生成するステップと、
を備え、
前記第２速度指令と前記第３速度指令との和を前記速度指令とすることを特徴とする数値制御方法。
【請求項７】
前記減速停止距離は、モータ速度を前記所定の減速度で減速させて停止させたときの減速停止距離とすることを特徴とする請求項６記載の数値制御方法。
【請求項８】
前記第３速度指令から所定の微小速度を減算して新たな第３速度指令とすることを特徴とする請求項６記載の数値制御方法。
【請求項９】
前記モータ位置は、前記速度指令を積分して得ることを特徴とする請求項６記載の数値制御方法。
【請求項１０】
前記モータ位置は、前記速度検出器の生成するモータ速度を積分して得ることを特徴とする請求項６記載の数値制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、可動体を速度制御する数値制御装置に関する。
【背景技術】
【０００２】
従来より、工作機械等の駆動を制御する数値制御においては、プログラムミスやオペレータの操作ミス等により制御対象の可動体が他の構成部品に干渉するのを防止するために、移動禁止領域を設定し、可動体がこの移動禁止領域に侵入したことを検出した時、警告を発すると共に可動体を非常停止させるように制御している。この移動禁止領域への侵入の検出方法として、パラメータによって可動上限値及び下限値を設定し、エンコーダ等の位置検出器からの可動体の現在位置信号と可動上限値及び下限値との比較によりソフト的に移動禁止領域への侵入を検出するソフトリミット手段が提案されてきた。
【０００３】
速度制御時のソフトリミット手段においては、位置制御時とは異なり可動体の目標位置の変更や指令位置の管理を行わない為、移動可能領域内に停止させることは困難である。速度制御時のソフトリミット手段の１つとして、特許文献１によれば、図４に示すように速度指令信号Ｖに従って移動中の可動体が移動可能領域を超えた時に、図４（Ａ）のソフトリミットアラームＳＡを発生させ、緊急停止の為に即座に速度指令信号Ｖを遮断する（図４（Ｂ））。しかし速度指令信号Ｖを遮断して速度指令信号Ｖを０にすると、停止時にモータや機械系に大きな衝撃を与えることになる為、図４（Ｃ）、（Ｄ）に示すように速度指令信号Ｖを遮断すると同時にモータへの電流指令信号Ｉを減少させて減速停止させることによりモータを少ない衝撃で停止させるソフトリミット手段が提案されている。
【特許文献１】特開平５−２５００２２号公報（第２、３頁、図４）
【発明の開示】
【発明が解決しようとする課題】
【０００４】
従来の速度制御時のソフトリミット手段は、位置制御時とは異なり可動体の目標位置の変更や指令位置の管理を行わないので移動可能領域内に停止させることが困難であることや、可動体が移動可能領域を超えた時に減速させて停止させるので可動体の減速停止距離の分がオーバートラベル量なる。また、減速停止させる手段として可動体へ出力する電流を制御する処理を必要とするので、制御系が複雑になると共に演算負荷も増大するというような問題があった。
【０００５】
本発明はこのような問題点に鑑みてなされたものであり、速度制御モードで速度指令値に従って可動体を移動させる場合、移動中の可動体が移動禁止領域に達するか否かを判断し、移動禁止領域に達すると判断した場合には、速度指令値を減速させた速度指令値に変更して可動体を減速させ、移動禁止領域の境界からのオーバートラベル量が少なく、停止時にモータや機械系に大きな衝撃を与えることなく安全に停止できる数値制御装置とその制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記問題を解決するため、本発明は、次のように構成したのである。
【０００７】
請求項１に記載の発明は、一定の制御時間ごとにアプリケーションプログラムを解析し位置制御モード時は位置指令を、速度制御モード時は第１速度指令を生成するプログラム解析手段と、前記位置指令とモータ位置との差から第２速度指令を生成する位置制御手段と、第１速度指令と第２速度指令の和を速度指令とし、前記速度指令と速度の差からトルク指令を生成する速度制御手段と、前記トルク指令から電流指令を生成し、前記電流指令と電流の差から電圧指令を生成してモータを駆動するモータ駆動手段と、前記電流を検出する電流検出手段と、モータに結合した位置検出器の生成するモータ位置をモータ速度に変換する位置速度変換手段と、前記可動体の移動禁止位置を設定するソフトリミット設定手段とを有し、前記モータを駆動して前記可動体を移動させる数値制御装置において、所定の減速度を設定する減速度設定手段と、前記第１速度指令の指令速度を前記所定の減速度で減速停止させたときの減速停止距離が、前記移動禁止位置と現在のモータ位置との差である移動可能距離に等しくなるように前記第１速度指令を変更し、第３速度指令を生成する移動禁止位置不可侵手段と、を備え、前記第２速度指令と前記第３速度指令との和を前記速度指令とすることを特徴とするものである。
請求項２に記載の発明は、請求項１記載の数値制御装置において、前記減速停止距離は、前記モータ速度を前記所定の減速度で減速させて停止させたときの距離とすることを特徴とするものである。
請求項３に記載の発明は、請求項１記載の数値制御装置において、前記移動禁止位置不可侵手段は、前記減速停止距離が前記移動可能距離よりも大きいときは前記第３速度指令から所定の微小速度を減算し新たな第３速度指令とすることを特徴とするものである。
請求項４に記載の発明は、請求項１記載の数値制御装置において、前記モータ位置は、前記速度指令を積分して得ることを特徴とするものである。
請求項５に記載の発明は、請求項１記載の数値制御装置において、前記モータ位置は、前記速度検出器の生成するモータ速度を積分して得ることを特徴とするものである。
請求項６に記載の発明は、一定の制御時間ごとにアプリケーションプログラムを解析し位置制御モード時は位置指令を生成、速度制御モード時は第１速度指令を生成するプログラム解析手段と、前記位置指令とモータ位置との差から第２速度指令を生成する位置制御手段と、第１速度指令と第２速度指令の和を速度指令とし、前記速度指令と速度の差からトルク指令を生成する速度制御手段と、前記トルク指令から電流指令を生成し、前記電流指令と電流の差から電圧指令を生成しモータを駆動するモータ駆動手段と、前記電流を検出する電流検出手段と、モータに結合した位置検出器の生成するモータ位置をモータ速度に変換する位置速度変換手段と、前記可動体の移動禁止位置を設定するソフトリミット設定手段とを有し、前記モータを駆動して可動体を移動させる数値制御方法において、運転前に、前記可動体の移動禁止位置をソフトリミットとして設定するステップと、所定の減速度を設定するステップと、運転時に、制御時間ごとに、前記第１速度指令の指令速度を前記所定の減速度で減速させて停止させたときの減速停止距離を演算するステップと、前記移動禁止位置と現在のモータ位置との差である移動可能距離を演算するステップと、前記減速停止距離が前記移動可能距離に等しくなるように前記第１速度指令を変更し第３速度指令を生成するステップと、を備え、前記第２速度指令と前記第３速度指令との和を前記速度指令とすることを特徴とするものである。
請求項７に記載の発明は、請求項６記載の数値制御方法において、前記減速停止距離は、モータ速度を前記所定の減速度で減速させて停止させたときの減速停止距離とすることを特徴とするものである。
請求項８に記載の発明は、請求項６記載の数値制御方法において、前記第３速度指令から所定の微小速度を減算して新たな第３速度指令とすることを特徴とするものである。
請求項９に記載の発明は、請求項６記載の数値制御方法にいて、前記モータ位置は、前記速度指令を積分して得ることを特徴とするものである。
請求項１０に記載の発明は、請求項６記載の数値制御方法において、前記モータ位置は、前記速度検出器の生成するモータ速度を積分して得ることを特徴とするものである。
【発明の効果】
【０００８】
本発明によると、ユーザアプリケーションプログラムにより与えられた速度指令値に従って可動体を移動させる数値制御装置において、速度指令値から可動体の位置を推定し、推定した可動体の位置から予め定められた減速度に従って移動禁止領域の境界で停止できるか否かを判断すると共に、停止できないと判断した場合には、速度指令値を減少させて可動体を移動禁止領域の境界近傍に停止させることができるので、移動禁止領域の境界からのオーバートラベル量が少なく、停止時にモータや機械系に大きな衝撃を与えることなく安全に停止できる。
【発明を実施するための最良の形態】
【０００９】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【００１０】
図１は、本発明のモーションシステムのハードウエア構成図である。図１において、１は接続された制御機器をコントロールするコントローラ、２ｎ（ｎ＝１、２、・・・）はコントローラ１から出力される指令に従って、接続されるサーボモータの回転駆動を制御するサーボドライブ、３ｎはサーボモータ、４はコントローラとサーボドライブ、サーボドライブとサーボモータを接続するモーションネットワーク、５は各種演算結果を出力するＣＰＵと設定された周期で定周期信号を出力する定周期発生回路と定周期信号を入力としてＣＰＵに割り込み信号を出力する割り込み回路を備えたＣＰＵユニット、６はユーザによって記述されたユーザアプリケーションプログラム７とソフトリミット処理が記述されたソフトリミットプログラム８や各種制御パラメータ等を記憶したＲＡＭ、９はオペレータ等が各種制御パラメータ等を入力する為の入力装置である。なお、コントローラ１は本来コントローラとして機能するための機能ブロックや通信処理を実行するための機能ブロックを当然備えているものであるが、本発明の説明には直接関係しないため図示していない。また、本実施例では予め入力装置９によってソフトリミット位置ＰＳと減速度ＤＣがオペレータ等によって設定されている。
【００１１】
図２は本発明の数値制御装置の機能構成を１軸に絞って示すブロック図である。図２において、５１は周期的にアプリケーションプログラムを解析して位置制御モードのときは位置指令を生成し、速度制御モードのときは第１速度指令を生成する。５２は位置制御手段で制御時間ごとに位置指令からモータ位置を減算して位置偏差を生成し、さらに位置偏差をＰＩＤ制御処理をして第２速度指令を生成する。５３は速度制御手段であり、制御時間ごとに速度指令らモータ速度を減算し速度偏差を生成、さらに速度偏差をＰＩＤ制御処理をしてトルク指令を生成する。５４はモータ駆動手段で、制御時間ごとにトルク指令を電流指令に変換し、次に電流指令から電流を減算して電流偏差を生成し、次に電流偏差をＰＩＤ処理して電圧指令を生成し、次に電圧指令を電力増幅してモータを駆動する。５５はモータ電流を検出する電流検出器であり電流信号を生成する。５６は位置速度変換手段で、直近の制御時間前後の位置差分を制御時間で除算して速度を生成する。５７はソフトリミット設定手段で、移動禁止領域の位置をパラメータで設定する。通常はハードのリミットスイッチの可動範囲内側に設定する。５８は減速度設定手段で、モータの最大トルクや可動体などから決る最大減速度よりも小さな値を設定する。５９は移動禁止位置不可侵手段で、第１速度指令の速度から設定減速度で減速させて停止させたときの設定減速停止距離が、可動禁止領域の禁止位置と現在の位置との差である移動可能距離よりも小さくなるように第１速度指令を変更し、第３速度指令を生成する。
【００１２】
図３は本発明の数値制御装置の数値制御方法を示すフローチャートである。ステップＳＴ１で可動体の移動禁止領域をソフトリミットパラメータで設定し、ステップＳＴ２でモータ最大トルクや可動体などから決る最大減速度よりも小さい減速度である設定減速度を設定する。これらはいずれも初期設定か、その都度、プログラミングパネルからオペレータによって設定される。次に制御時間ごとの定時割込みによるフローチャートで、ステップＳＴ３で、第速度指令の速度から設定減速度で減速したときの設定減速停止距離を演算、ステップＳＴ４でソフトリミット位置から現在位置を減算して停止可能距離を演算する。次にステップＳＴ５で設定減速停止距離と停止可能距離を比較し、設定減速停止距離が大きければステップＳＴ５に移動し、小さければ終了して次の制御時間を待つ。ステップＳＴ５では第１速度指令を第３速度指令に変更して次の制御時間を待つ。
【００１３】
図４は速度指令を積分することによってモータ位置を推定しソフトリミット処理をする場合である。図４において、ｄｔの指令周期毎に発生するソフトウェア割り込みによって起動されるソフトリミットプログラム８において、サーボモータ３ｎの移動可能距離と減速停止距離を求めると共に距離の比較を行い、ソフトリミット位置に達すると判断した場合には予め定められた減速度で停止するまでの処理手順を示すフローチャートである。また、図５はユーザアプリケーションプログラム６でプログラムされている速度指令値に従って移動中のサーボモータ３ｎがソフトリミット位置に達すると判断され、減速停止するまでのタイミングチャートを示している。はじめにソフトウェア割り込みによってソフトリミットプログラム８が起動し、図４のＳ１００１でソフトリミットアラームフラグがＯＮ状態であるか否かをチェックし、ＯＮ状態でなければ、Ｓ１００２でユーザアプリケーションプログラム６で指令される速度指令値Ｖと予め定められた減速度ＤＣから、サーボモータ３ｎが減速停止するまでに要する減速停止距離ＤＬを次式のように求める。
【００１４】
ＤＬ＝Ｖ２／（２・ＤＣ）                                  （１）
次にＳ１００３において、図５（Ａ）に示すように今回指令周期の時刻をＴｉ（ｉは任意）とすると、速度指令Ｖに従って移動するサーボモータ３ｎの次回指令周期での位置ＰＮ（つまり次回指令周期の時刻Ｔｉ＋１での位置）を推定する。ここで推定現在位置をＰＣとすると、推定次回位置ＰＮは次式のよう速度指令を積分して推定する。
【００１５】
ＰＮ＝ＰＣ＋Ｖ・ｄｔ                                    （２）
なお、推定次回位置ＰＮは時刻Ｔｉ＋１でのＳ１００３で推定現在位置ＰＣとして使用する為、推定現在値ＰＣを推定次回位置ＰＮで更新する。
【００１６】
Ｓ１００４は、時刻Ｔｉ＋１におけるサーボモータ３ｎがソフトリミット位置ＰＳまで移動することができる移動可能距離ＡＬを求める。移動可能距離ＡＬは、予め定められたソフトリミット位置ＰＳとＳ１００２で求めた推定次回位置ＰＮの差分値である。
【００１７】
ＡＬ＝ＰＳ−ＰＮ                                        （３）
次にＳ１００５でＳ１００２、Ｓ１００４で求めた減速停止距離ＤＬと移動可能距離ＡＬとを比較し、減速停止距離ＤＬが移動可能距離ＡＬより大きい場合は、減速度ＤＣで減速停止させるとソフトリミット位置を超えて停止することになるので、今回指令周期の速度指令値Ｖでサーボモータ３ｎを移動させるとソフトリミット位置ＰＳに達すると判断される。ソフトリミット位置ＰＳに達すると判断された場合は、Ｓ１００６で図５（Ｃ）に示すように減速度ＤＣ分だけ速度指令値Ｖを減速させて、次式のように速度指令値Ｖを更新する。
【００１８】
Ｖ＝Ｖ−ＤＣ・ｄｔ                                      （４）  
本発明が特許文献１と異なる部分は、可動体がソフトリミット位置を超えた時に停止させるのではなく、次回指令周期における可動体の推定位置とソフトリミット位置までの移動可能距離を監視して、今回指令周期の速度指令値Ｖで可動体を移動させると予め指定された減速度でソフトリミット位置に停止することができないと判断した場合に、速度指令値Ｖを指定された減速度で減少させた速度指令値によって更新することにより、ソフトリミット位置に停止させる手段としている部分である。
【００１９】
次にＳ１００７において、今回指令周期の速度指令値Ｖで減速停止が完了するか否かを判断し、減速停止が完了する場合には、ソフトリミットアラームフラグをＯＦＦにし、減速停止が完了しない場合には、図５（Ｂ）のようにソフトリミットアラームフラグをＯＮにしてソフトリミットプログラム８を終了する。
なお、ソフトリミットアラームフラグがＯＮ状態であれば、次回指令周期のＳ１００１において減速停止が完了していないと判断され、Ｓ１００６で再び速度指令値Ｖが更新される。
【００２０】
ソフトリミットプログラム８終了後に、速度指令値Ｖは、下位サーボドライブ２ｍに送信される。これらの一連の処理フローをｄｔの指令周期毎に行う。
【００２１】
現在位置であるモータ位置を得るのに、速度検出器をモータに結合し、速度検出器の生成するモータ速度を時間積分することもできる。
【００２２】
図７は従来技術を、図８は本発明の動作をシミュレーションしたものである。条件は、可動子換算全質量Ｍ＝１０ｋｇ、モータ最大推力Ｆｍａｘ＝５００Ｎ、設定減速度ＤＣＣ＝３０ｍ／ｓ２、モータ誘起電圧定数Ｋｅ＝１３.３Ｖｓ／ｍ、速度制御比例ゲインＫｖ＝１００００Ｎｓ／ｍ、速度制御積分時定数Ｔｖｉ＝２０ｍｓ、推力フィルタＴｆ＝０．２ｍｓ、一定速度指令Ｖ＝０．５ｍ／ｓ、ソフトリミット設定Ｘｌｉｍ＝０．０８５ｍ、加速時間Ｔａｃ＝５０ｍｓである。図７では、モータ位置がソフトリミット位置まで達するまで、一定速度指令Ｖ＝０．５ｍ／ｓで走行し、ソフトリミット位置に達したら速度指令Ｖ＝０ｍ／ｓとし、最大減速度で減速する。電流が最大電流に達するため、速度制御ループは完全に飽和し、ソフトリミットを超えて行過ぎた分の速度偏差は速度制御積分に蓄積され、その後、放出されるために再びソフトリミット位置までもどってくる。図８では、モータ速度がソフトリミット位置に近づくと速度指令をソフトリミット位置で減速停止できる値に減少させるのでソフトリミット位置を行き過ぎることはない。（図７、図８の丸印部分）
【００２３】
このように、指令周期毎に速度指令値と減速度からソフトリミット位置までの移動可能距離と減速距離を求めると共に比較を行い、予め定められた減速度でソフトリミット位置に停止できないと判断した場合には、減速度に従って指令速度を減少させてソフトリミット位置の境界近傍に停止させることができるので、ソフトリミット位置からのオーバートラベル量も少なく、また停止時にモータや機械系に大きな衝撃を与えることなく安全に停止させることができる。
さらに、減速停止させる為に電流制御等の複雑な制御系を必要とせず、簡単な演算のみで減速停止させることができるので、電流制御回路等のハードウェアの付加や演算負荷の増大がなく停止させることができる。
【産業上の利用可能性】
【００２４】
速度指令によってモータを制御するモーションコントローラの用途などに適用できる。
【図面の簡単な説明】
【００２５】
【図１】本発明の実施例を示すモーションシステム構成図
【図２】本発明の機能構成を示すブロック図
【図３】本発明の制御方法を示すフローチャート
【図４】ソフトリミット処理フローチャート
【図５】本発明のソフトリミット処理タイミングチャート
【図６】従来技術のソフトリミット処理タイミングチャート
【図７】従来技術のシミュレーション
【図８】本発明のシミュレーション
【符号の説明】
【００２６】
１  コントローラ
２ｎ（ｎ＝１、２、・・・）  サーボドライブ
３ｎ（ｎ＝１、２、・・・）  サーボモータ
４  モーションネットワーク
５  ＣＰＵユニット
６  ＲＡＭ
７  ユーザアプリケーションプログラム
８  ソフトリミットプログラム
９  入力装置
５１  プログラム解析手段
５２  位置制御手段
５３  速度制御手段
５４  モータ駆動手段
５５  電流検出手段
５６  位置速度変換手段
５７  ソフトリミット設定手段
５８  減速度設定手段
５９  移動禁止位置不可侵手段
７３  位置検出器
７４  可動体
【図１】
2007052590.tif 000003
【図２】
2007052590.tif 000004
【図３】
2007052590.tif 000005
【図４】
2007052590.tif 000006
【図５】
2007052590.tif 000007
【図６】
2007052590.tif 000008
【図７】
2007052590.tif 000009
【図８】
2007052590.tif 000010