(57)【特許請求の範囲】
【請求項１】
位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置の制御方法において、
指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求め、
トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とする、
という手順で処理することを特徴とするデジタルサーボ制御装置の制御方法。
【請求項２】
位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置の制御方法において、
減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求め、
トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とする、
という手順で処理することを特徴とするデジタルサーボ制御装置の制御方法。
【請求項３】
位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置の制御方法において、
指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求め、
トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とし、
減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求め、
トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とする、
という手順で処理することを特徴とするデジタルサーボ制御装置の制御方法。
【請求項４】
前記加速が終了するサンプリングｋａｅｎｄを求める計算では以下の計算式を用いることを特徴とする請求項１およびまたは３のいずれかに記載のデジタルサーボ制御装置の制御方法。
ｋａｅｎｄ  ＝  ｋ０＋ｖｍａｘ／ｄｖａ  −  １
ｋ０：加速開始指令サンプリング
【請求項５】
前記減速が終了するサンプリングｋｄｅｎｄを求める計算では以下の計算式を用いることを特徴とする請求項２または３のいずれかに記載のデジタルサーボ制御装置の制御方法。
ｋｄｅｎｄ  ＝  ｎ０＋ｖｄ／ｄｖｄ  −  １
ｎ０：減速指令開始サンプリング
【請求項６】
指令払い出し開始時に前記最大速度ｖｍａｘの値を上位装置から取得することを特徴とする請求項１乃至３のいずれかに記載のデジタルサーボ制御装置の制御方法。
【請求項７】
前記トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓが小数の場合、ｋａｅｎｄ−ｓの小数点以下を切り捨てたサンプリング時は加速時のトルクフィードフォワードの値信号を適当な整数α分の１とし、小数点以下を切り上げたサンプリング時は加速時のトルクフィードフォワード信号を０とするという手順で処理することを特徴とする請求項１または３のいずれかに記載のデジタルサーボ制御装置の制御方法。
【請求項８】
前記適当な整数αをｋａｅｎｄ−ｓの小数点以下の値の逆数の小数点以下を切り捨てた値とすることを特徴とする請求項７記載のデジタルサーボ制御装置の制御方法。
【請求項９】
前記トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓが小数の場合、ｋｄｅｎｄ−ｓの小数点以下を切り捨てたサンプリング時は減速時のトルクフィードフォワードの値信号を適当な整数α分の１とし、小数点以下を切り上げたサンプリング時は減速時のトルクフィードフォワード信号を０とするという手順で処理することを特徴とする請求項２または３のいずれかに記載のデジタルサーボ制御装置の制御方法。
【請求項１０】
前記適当な整数αの値をｋｄｅｎｄ−ｓの小数点以下の値の逆数の小数点以下を切り捨てた値とすることを特徴とする請求項９記載のデジタルサーボ制御装置の制御方法。
【請求項１１】
位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置において、
指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求める加速終了時予測部と、
トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とするトルクフィードフォワード信号補正部と、
を有することを特徴とするデジタルサーボ制御装置。
【請求項１２】
位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置において、
減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求める減速終了時予測部と、
トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とするトルクフィードフォワード信号補正部と、
を有することを特徴とするデジタルサーボ制御装置。
【請求項１３】
位置制御手部、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置において、
指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求める加速終了時予測部と、
減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求める減速終了時予測部と、
トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とし、且つ、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とするトルクフィードフォワード信号補正部と、
を有することを特徴とするデジタルサーボ制御装置。
【請求項１４】
速度制御部と、電流制御部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置において、
指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求める加速終了時予測部と、
減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求める減速終了時予測部と、
トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とし、且つ、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とするトルクフィードフォワード信号補正部と、
を有することを特徴とするデジタルサーボ制御装置。
【請求項１５】
位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置の制御方法において、
加速終了時のサンプリングｋａｅｎｄおよび減速終了時のサンプリングｋｄｅｎｄを、上位装置から受け取り、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とし、且つ、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とする、
という手順で処理することを特徴とするデジタルサーボ制御装置の制御方法。
【請求項１６】
前記加速終了時のサンプリングｋａｅｎｄおよび減速終了時のサンプリングｋｄｅｎｄを直接受け取る代わりに、加速が終了する時間ｋａｔｉｍｅおよび減速が終了する時間ｋｄｔｉｍｅの値を上位装置から受け取り、それらをサンプリング周期Ｔｓで除算して、加速終了時のサンプリングｋａｅｎｄおよび減速終了時のサンプリングｋｄｅｎｄを算出することを特徴とする請求項１５記載のデジタルサーボ制御装置の制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、トルクフィードフォワードを使用し、デジタル制御時の位置決め性能を向上したデジタルサーボ制御装置及びその制御方法に関する。
【背景技術】
【０００２】
従来のサーボ制御方法は、位置指令を微分して速度フィードフォワード信号とし、もう一度微分してトルクフィードフォワード信号を作成し、そのまま使用していた。（例えば、特許文献１参照）。
第５図は従来のサーボ制御装置のブロック線図である。第５図中、伝達関数３０のＫＰは位置ループにおけるポジションゲイン、伝達関数３２のＫ１は速度ループにおける積分ゲイン、伝達関数３４のｋ２は速度ループにおける比例ゲイン、３６は電流ループ回路、３８はサーボモータの電気部で、Ｒは巻線の抵抗、Ｌは巻線のインダクタンス、４０はサーボモータの機械部で、Ｋｔはトルク定数、Ｊｍはイナーシャ、４２はサーボモータの回転速度を積分し、位置を算出する伝達関数である。また、伝達関数４４は位置のフィードフォワード項であり、α１は位置のフィードフォワード係数である。４６は速度のフィードフォワード項であり、α２は速度のフィードフォワード係数である。上記速度フィードフォワード係数α２は通常、Ｊｍ／Ｋｔ（Ｊｍ：イナーシャ、Ｋｔ：トルク定数）の値に近い値がとられる。なお、位置のフィードフォワード項４４のフィードフォワード係数α１の値はモータの特性等に合わせ実験的に決められる（理想的には「１」がよい）。
【０００３】
従来の制御では、位置指令ａを微分し、その微分値に位置のフィードフォワード係数α１を乗じて位置のフィードフォワード制御量とし、通常の位置ループ制御、即ち、位置指令ａからモータの現在位置Ｐを減じて位置偏差εを求め、これに位置ループゲインＫＰを乗じて通常の速度指令を求める。そして、この通常の速度指令に位置のフィードフォワード制御量を加算し、位置のフィードフォワード制御が行われた位置指令Ｖｃを求める。
一方、位置のフィードフォワード制御量を微分し、速度のフィードフォワード係数α２を乗じ、速度のフィードフォワード制御量を求め、かつ、速度ループ制御（ＩＰ制御）、即ち、速度指令Ｖｃからサーボモータの実速度Ｖを減じて速度偏差を求め、該速度偏差を積分し積分ゲインｋ１を乗じた値からサーボモータの実速度Ｖに比例ゲインｋ２を掛けた値を減じて得られる従来の電流指令値に、上記速度フィードフォワード制御量を加算し電流指令Ｉｃを求める。
【０００４】
位置指令ａの値が変化すれば位置偏差εも大きくなり、通常の位置ループ処理で出力される速度指令も大きく変化するが、位置ループ処理の遅れがある。しかし、位置のフィードフォワード制御によって、位置指令ａの変化量に応じて位置のフィードフォワード制御量も増大して速度指令に加算され、フィードフォワード制御された速度指令となるから、位置ループの遅れは補償される。また、速度ループも同様で、速度指令の変化に応じて、通常の速度ループ処理による電流指令も変化するが、積分項があるため遅れが生じる。しかし、この場合も、速度のフィードフォワード制御によって速度のフィードフォワード制御量が加算され電流指令となるから、速度ループの遅れも補償され、全体としてサーボ系の応答は向上する。その結果、位置指令ａに対するサーボモータの追従性が良くなり、位置偏差のうねりは軽減されることとなる。
【０００５】
従来の制御をデジタル制御で実現する際は以下の方法がとられていた。
位置，速度ループ処理の周期をＴＰとし、各位置，速度ループ処理における位置指令をａ（ｎ）（ｎ＝１，２，３……で、ｎ≦０ではａ（ｎ）＝０）とすると、位置指令ａ（ｎ）の微分値ｂ（ｎ）は実際には差分として次の第（１）式の演算によって算出される。
【０００６】
ｂ（ｎ）＝｛ａ（ｎ）−ａ（ｎ−１）｝／Ｔｐ                     ・・・（１）
【０００７】
位置のフィードフォワード信号は、上記ｂ（ｎ）の値に位置のフィードフォワード係数α１を乗じてフィードフォワード制御量ｃ（ｎ）としていた。
【０００８】
ｃ（ｎ）＝α１・ｂ（ｎ）                                 ・・・（２）
【０００９】
速度のフィードフォワード信号Ｄｎは、式（３）に示すように、位置のフィードフォワード制御量ｃ（ｎ）から前周期の位置のフィードフォワード制御量ｃ（ｎ−１）を減じ、得られた値に（α２／ＴＰ）を乗じて求めていた。
【００１０】
ｄ（ｎ）＝α２・｛（ｃ（ｎ）−ｃ（ｎ−１）｝／Ｔｐ              ・・・（３）
【００１１】
このように、従来のフィードフォワード制御装置では、単純に位置指令を微分（今回値と前回値の差をサンプリング周期で除算）したものにα１を乗じたものを速度フィードフォワード信号とし、もう一度微分したものにα２を乗じたものを電流（あるいはトルク）フィードフォワード信号とするという手順がとられていた。
【特許文献１】特許第２７６２３６４号公報（第４−７頁、図１、図８）
【発明の開示】
【発明が解決しようとする課題】
【００１２】
従来のサーボ制御方法では、単純に位置指令を微分（今回値と前回値の差をサンプリング周期で除算）したものにα１を乗じたものを速度フィードフォワード信号とし、もう一度微分したものにα２を乗じたものを電流（あるいはトルク）フィードフォワード信号とするという手順をとっているので、理想的には、速度指令終了後、次の１サンプリング間は、速度フィードフォワード信号が０であるため、電流フィードフォワード信号も０である必要があるのに、０次ホールドのせいで、図３（ｂ）の斜線部分に示すようにトルクフィードフォワード信号が１サンプリング分多めに出力されてしまい、結果として過補償となり、オーバシュートや位置偏差の増大を招いてしまうという問題があった。
また、実際には制御演算の遅れが１サンプリング分あるため、図３（ｃ）の斜線部に示すように２サンプリング分余分にトルクフィードフォワード信号が出てることになったり、さらに、制御演算の遅れ以外に差分近似の影響でさらに０．５サンプリング時間遅れが生じるため、図４（ｄ）の斜線部に示すように２．５サンプリング分余分にトルクフィードフォワード信号が出てることになり、結果としてさらに過補償となり、オーバシュートや位置偏差の増大を招いてしまうという問題があった。
【００１３】
本発明はこのような問題点に鑑みてなされたものであり、トルクフィードフォワード信号の過補償を防ぎ、加減速終了時および位置決め時の偏差が非常に小さく、しかも計算が簡単でどのような指令にも対応できるデジタルサーボ制御装置及びその制御方法を提供することを目的とする。
【課題を解決するための手段】
【００１４】
上記問題を解決するため、本発明は、次のようにしたのである。
請求項１に記載の発明は、位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置の制御方法において、指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求め、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とする、という手順で処理することを特徴とするものである。
【００１５】
請求項２に記載の発明は、位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置の制御方法において、減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求め、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とする、という手順で処理することを特徴とするものである。
【００１６】
請求項３に記載の発明は、位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置の制御方法において、指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求め、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とし、減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求め、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とする、という手順で処理することを特徴とするものである。
【００１７】
請求項４に記載の発明は、請求項１または３のいずれかに記載のデジタルサーボ制御装置の制御方法において、前記加速が終了するサンプリングｋａｅｎｄを求める計算では以下の計算式を用いること特徴とするものである。
【００１８】
ｋａｅｎｄ  ＝  ｋ０＋ｖｍａｘ／ｄｖａ  −  １
ｋ０：加速開始指令サンプリング
【００１９】
請求項５に記載の発明は、請求項２または３のいずれかに記載のデジタルサーボ制御装置の制御方法において、前記減速が終了するサンプリングｋｄｅｎｄを求める計算では以下の計算式を用いること特徴とするものである。
ｋｄｅｎｄ  ＝  ｎ０＋ｖｄ／ｄｖｄ  −  １
ｎ０：減速指令開始サンプリング
【００２０】
請求項６に記載の発明は、請求項１乃至３のいずれかに記載のデジタルサーボ制御装置の制御方法において、前記最大速度ｖｍａｘの値を指令払い出し開始時に上位装置から取得することを特徴とするものである。
【００２１】
請求項７に記載の発明は、請求項１または３のいずれかに記載のデジタルサーボ制御装置の制御方法において、前記トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓが小数の場合、ｋａｅｎｄ−ｓの小数点以下を切り捨てたサンプリング時は加速時のトルクフィードフォワードの値信号を適当な整数α分の１とし、小数点以下を切り上げたサンプリング時は加速時のトルクフィードフォワード信号を０とするという手順で処理することを特徴とするものである。
【００２２】
請求項８に記載の発明は、請求項７記載のデジタルサーボ制御装置の制御方法において、前記適当な整数αをｋａｅｎｄ−ｓの小数点以下の値の逆数の小数点以下を切り捨てた値とすることを特徴とするものである。
【００２３】
請求項９に記載の発明は、請求項２または３のいずれかに記載のデジタルサーボ制御装置の制御方法において、前記トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓが小数の場合、ｋｄｅｎｄ−ｓの小数点以下を切り捨てたサンプリング時は減速時のトルクフィードフォワードの値信号を適当な整数α分の１とし、小数点以下を切り上げたサンプリング時は減速時のトルクフィードフォワード信号を０とするという手順で処理することを特徴とするものである。
【００２４】
請求項１０に記載の発明は、請求項９記載のデジタルサーボ制御装置の制御方法において、前記適当な整数αの値をｋｄｅｎｄ−ｓの小数点以下の値の逆数の小数点以下を切捨てた値とすることを特徴とするものである。
【００２５】
請求項１１に記載の発明は、位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置において、指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求める加速終了時予測部と、トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とするトルクフィードフォワード信号補正部と、を有することを特徴とするものである。
【００２６】
請求項１２に記載の発明は、位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置において、減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求める減速終了時予測部と、トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とするトルクフィードフォワード信号補正部と、を有することを特徴とするものである。
【００２７】
請求項１３に記載の発明は、位置制御手部、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置において、指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求める加速終了時予測部と、減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求める減速終了時予測部と、トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とし、且つ、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とするトルクフィードフォワード信号補正部と、を有することを特徴とするものである。
【００２８】
請求項１４に記載の発明は、速度制御部と、電流制御部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置において、指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求める加速終了時予測部と、減速開始１回目のサンプリング時の速度指令の減分値ｄｖｄと減速開始時の速度ｖｄを用いて、減速が終了するサンプリングｋｄｅｎｄを計算で求める減速終了時予測部と、トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とし、且つ、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とするトルクフィードフォワード信号補正部と、を有することを特徴とするものである。
【００２９】
請求項１５に記載の発明は、位置制御部と、速度制御部と、電流制御部と、速度フィードフォワード作成部と、トルクフィードフォワード作成部を備えたデジタルサーボ制御装置の制御方法において、加速終了時のサンプリングｋａｅｎｄおよび減速終了時のサンプリングｋｄｅｎｄを、上位装置から受け取り、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とし、且つ、（ｋｄｅｎｄ  ―  ｓ）サンプリング以降の減速時のトルクフィードフォワード信号を強制的に０とする、という手順で処理することを特徴とするものである。
【００３０】
請求項１６に記載の発明は、請求項１５記載のデジタルサーボ制御装置の制御方法において、前記加速終了時のサンプリングｋａｅｎｄおよび減速終了時のサンプリングｋｄｅｎｄを直接受け取る代わりに、加速が終了する時間ｋａｔｉｍｅおよび減速が終了する時間ｋｄｔｉｍｅの値を上位装置から受け取り、それらをサンプリング周期Ｔｓで除算して、加速終了時のサンプリングｋａｅｎｄおよび減速終了時のサンプリングｋｄｅｎｄを算出することを特徴とするものである。
【発明の効果】
【００３１】
請求項１および請求項１１に記載の発明によると、加速終了時点が正確に分かり、その時に余分なトルクフィードフォワード信号を出力しなくなるため、過補償を避けることができ、加速終了時の偏差を小さくできる。
また、請求項２および請求項１２に記載の発明によると、減速終了時点が正確に分かり、その時に余分なトルクフィードフォワード信号を出力しなくなるため、過補償を避けることができ、減速終了時の偏差を小さくできる。
また、請求項３および請求項１３に記載の発明によると、加速終了時点と減速終了時点が正確に分かり、その時に余分なトルクフィードフォワード信号を出力しなくなるため、過補償を避けることができ、加速終了時および減速終了時の両方の偏差を小さくできる。
また、請求項４に記載の発明によると、台形指令などのように一定加速する指令時に、簡単な計算で加速終了時点を予測することができ、加速終了時の偏差を小さくできる。
また、請求項５に記載の発明によると、台形指令などのように一定減速する指令時に、簡単な計算で減速終了時点を予測することができ、減速終了時の偏差を小さくできる。
また、請求項６に記載の発明によると、上位装置からｖｍａｘを取得するため、予め指令の最大速度ｖｍａｘが既知でなくても、どのような指令に対しても本発明を適用することができる。
また、請求項７に記載の発明によると、前記トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓが小数の場合も、その時に余分なトルクフィードフォワード信号を出力しなくなるため、過補償を避けることができ、加速終了時の偏差を小さくできる。
また、請求項９に記載の発明によると、前記トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓが小数の場合も、その時に余分なトルクフィードフォワード信号を出力しなくなるため、過補償を避けることができ、減速終了時の偏差を小さくできる。
また、請求項８および１０に記載の発明によると、前記トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓが小数の場合に、整数αの値をｋｄｅｎｄ−ｓの小数点以下の値の逆数とするので、簡単にαを設定でき、ｓが小数の時も簡単に加速終了時と減速終了時の偏差を小さくできる。
また、請求１４に記載の発明によると、デジタル制御装置が速度制御を行う時も加速終了時と減速終了時の偏差を小さくできる。
また、請求１５および１６に記載の発明によると、直接、加速終了時のサンプリングｋａｅｎｄおよび減速終了時のサンプリングｋｄｅｎｄ相当の情報が得られるため、少しの演算で、加速終了時と減速終了時の偏差を小さくできる。
【発明を実施するための最良の形態】
【００３２】
以下、本発明の方法の具体的実施例について、図に基づいて説明する。
【実施例１】
【００３３】
図１は、本発明の方法を実施するデジタルサーボ制御装置の構成を示すブロック図である。図において１は制御対象であり、２は制御対象の位置および速度を検出する検出器を表す。１００は本発明のデジタルサーボ制御装置を表す。位置指令ｒｅｆと、制御対象に取り付けた検出器２で検出した位置検出値ｘを入力して、制御演算をデジタル演算し、電流Ｉをモータに出力する。
３は位置制御部、４は速度制御部、５は電流制御部を表し、それぞれの制御部は比例あるいは比例積分制御を行う。６は速度フィードフォワード作成部を表し、位置指令を微分し速度フィードフォワード信号ｖｆｆを作成する。７はトルクフィードフォワード作成部を表し、速度フィードフォワード信号ｖｆｆをさらに微分し制御対象のイナーシャを乗じてトルクフィードフォワード信号ｔｆｆを作成する。
８は加速終了時予測部を表し、上位装置からｖｍａｘと指令を受け取り、指令払い出し開始のサンプリングから数えていくつ先のサンプリング時に加速が終了するかを計算により求め加速終了時のサンプリングｋａｅｎｄを出力する。ここで、指令が既知の場合には、ｖｍａｘの値は上位装置からもらうのではなく、予めメモリに格納しておいても良い。
９は減速終了時予測部を表し、減速開始時の速度ｖｄと指令から、減速開始のサンプリングから数えていくつ先のサンプリング時に減速が終了するかを計算により求め減速終了時のサンプリングｋｄｅｎｄを出力する。
１０はトルクフィードフォワード信号補正部を表し、トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、ｓとｋａｅｎｄとｋｄｅｎｄとｔｆｆを入力し、トルクフィードフォワード信号が過補償とならないようにトルクフィードフォワード信号ｔｆｆを補正する。ここで、ｓは予めメモリに格納しておいても良いし、パラメータとしておいて調整しても良い。１１〜１３は演算部である。
【００３４】
次に、各部ごとの、デジタル演算の方法について述べる。
以下、サンプリング時間はＴｓ、今回のサンプリングをｋ番目、ｕ回前のサンプリングをｋ−ｕと定義し、変数ｑのｋ−ｕ番目のサンプリング時の値をｑ（ｋ−ｕ）と表記することにする。
【００３５】
まず、位置制御部３では、式（４）に表すように位置指令ｒｅｆ（ｋ）と位置検出値ｘ（ｋ）の差に位置ループゲインＫｐを乗じる計算を行い速度指令ｖｒｅｆ（ｋ）を出力する。
【００３６】
ｖｒｅｆ（ｋ）＝Ｋｐ・｛ｒｅｆ（ｋ）−ｘ（ｋ）｝               ・・・（４）
【００３７】
次に、速度制御部４では、式（５）に表すように、速度指令ｖｒｅｆ（ｋ）と速度検出値ｖ（ｋ）の差に速度ＦＦ作成部の出力である速度フィードフォワード信号ｖｆｆ（ｋ）を加算した値に速度ループ比例ゲインＫｖを乗じｓｒｅｆ（ｋ）を算出する。
【００３８】
ｓｒｅｆ（ｋ）＝Ｋｖ・｛ｖｒｅｆ（ｋ）−ｖ（ｋ）＋ｖｆｆ（ｋ）｝・・・（５）
【００３９】
ここで、ｖ（ｋ）は位置検出値ｘ（ｋ）の差分近似による微分で求めても良い。
次に、式（６）のように、ｓｒｅｆ（ｋ）に積分制御ゲインＫｉおよびサンプリング時間Ｔｓを乗じ、前回値に加算することにより積分演算を行いｓｉ（ｋ）を算出する。
【００４０】
ｓｉ（ｋ）＝ｓｉ（ｋ−１）＋Ｔｓ・Ｋｉ・ｓｒｅｆ（ｋ）         ・・・（６）
【００４１】
次に、式（７）のように、ｓｒｅｆ（ｋ）とｓｉ（ｋ）を加算することでトルク指令ｔｒｅｆ（ｋ）を算出する。
【００４２】
ｔｒｅｆ（ｋ）＝ｓｒｅｆ（ｋ）＋ｓｉ（ｋ）                    ・・・（７）
【００４３】
次に、電流制御部５では、トルク指令ｔｒ（ｋ）とトルクフィードフォワード信号ｔｆｆ（ｋ）を加算したものを入力し、単位変換および制御演算を行い電流値Ｉ（ｋ）を計算する。ここで電流制御部内部の処理は実際には交流を直流に変換するなど複雑な処理が行われるが、電流制御手段内部の処理方法は本発明に全く関係がなく、どのような処理をしても良いためここでは説明を省略する。
【００４４】
速度ＦＦ作成部６では、式（８）に示すように、位置指令ｒｅｆ（ｋ）を差分近似で微分し、速度フィードフォワード信号ｖｆｆ（ｋ）を算出する。
【００４５】
ｖｆｆ（ｋ）＝｛ｒｅｆ（ｋ）−ｒｅｆ（ｋ−１）｝／Ｔｓ        ・・・（８）
【００４６】
トルクＦＦ作成部７では、式（９）に示すように。速度フィードフォワード信号ｖｆｆ（ｋ）を差分近似で微分し、イナーシャＪｎを乗じてトルクフィードフォワード信号ｔｆｆ（ｋ）を算出する。
【００４７】
ｔｆｆ（ｋ）＝Ｊｎ・｛ｖｆｆ（ｋ）−ｖｆｆ（ｋ−１）｝／Ｔｓ    ・・・（９）
【００４８】
加速終了時予測部８では、式（１０）に示すように、指令払い出し開始時ｋ０回目のサンプリングの速度の増分値ｄｖａを求め、式（１１）に示すように、上位指令器から受け取ったｖｍａｘとｄｖａから加速終了時のサンプリングｋａｅｎｄを出力する。
【００４９】
ｄｖａ＝｛ｒｅｆ（ｋ０）−ｒｅｆ（ｋ０−１）｝／Ｔｓ
―｛ｒｅｆ（ｋ０−１）−ｒｅｆ（ｋ０−２）｝／Ｔｓ  ・・・（１０）
【００５０】
ｋａｅｎｄ＝ｋ０  ＋  ｖｍａｘ／ｄｖａ  −  １                ・・・（１１）
【００５１】
さらに図３を用いて詳細に説明する。
図３（ａ）が、上位から入力される位置指令を差分近似により微分した指令速度である。ｋ０は指令開始時のサンプリングを表す。本実施例では最高速度ｖｍａｘはｄｖａの４倍の大きさとなっている。したがって、式（１１）より、
【００５２】
ｋａｅｎｄ  ＝  ｋ０  ＋  ４  −  １  ＝  ｋ０  ＋  ３        ・・・（１２）
となり、加速が終了する時点のサンプリングはｋ０＋３となる。
【００５３】
トルクＦＦ信号補正部１０では、サンプリング（ｋａｅｎｄ  −  ｓ）以降のトルクフィードフォワードを強制的に０にする処理を行う。
【００５４】
図３（ｂ）で、斜線部は、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓを０サンプリングとした時に、余分となるトルクフィードフォワード信号である。この場合、
【００５５】
ｋａｅｎｄ−ｓ  ＝  ｋ０＋３−０  ＝  ｋ０＋３
以降のトルクフィードフォワード信号（斜線部のトルク）を強制的に０にする。
【００５６】
図３（ｃ）で、斜線部は、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓを１サンプリングとした時に、余分となるトルクフィードフォワード信号である。この場合、
【００５７】
ｋａｅｎｄ−ｓ  ＝  ｋ０＋３−１  ＝  ｋ０＋２
以降のトルクフィードフォワード信号（斜線部のトルク）を強制的に０にする。
【００５８】
図３（ｄ）で、斜線部は、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓを１．５サンプリングとした時に、余分となるトルクフィードフォワード信号である。この場合、
【００５９】
ｋａｅｎｄ−ｓ  ＝  ｋ０＋３−１．５  ＝  ｋ０＋１．５
以降のトルクフィードフォワード信号（斜線部のトルク）を強制的に０にする。
【００６０】
このように、計算結果が整数でない場合は、ｋａｅｎｄ−ｓの小数点以下を切り捨てたサンプリング時にトルクフィードフォワード信号を１／αとし、小数点以下を切り上げたサンプリング時はトルクフィードフォワード信号を０とするというようにすることで対応できる。
【００６１】
すなわち、図３（ｄ）のような場合は、１．５の小数点以下を切り捨てたｋ０＋１回目のサンプリングにはトルクフィードフォワード信号を１／αとして、小数点以下を切り上げたｋ０＋２のサンプリング時はトルクフィードフォワード信号を０とするようにすればよい。
【００６２】
また、αは調整により求めても良いが、小数点以下の値の逆数にしても良い。すなわち、図３（ｄ）のような場合は、小数点以下の値は０．５なのでα＝１／０．５＝２とすればよい。
【００６３】
次に、減速時の処理について説明する。減速時の処理も基本的に上で説明した加速時の処理と同じ方法である。
【００６４】
減速終了時予測部９では、式（１３）に示すように、減速開始時ｎ０回目のサンプリングの速度の減分値ｄｖｄを求め、式（１４）に示すように、減速開始時の速度ｖｄとｄｖｄから減速終了時のサンプリングｋｄｅｎｄを出力する。
【００６５】
ｄｖｄ＝｛ｒｅｆ（ｎ０）−ｒｅｆ（ｎ０−１）｝／Ｔｓ
―｛ｒｅｆ（ｎ０−１）−ｒｅｆ（ｎ０−２）｝／Ｔｓ  ・・・（１３）
【００６６】
ｋｄｅｎｄ＝ｎ０  ＋  ｖｄ／ｄｖｄ  −  １                    ・・・（１４）
【００６７】
さらに図３を用いて詳細に説明する。図３（ａ）が、上位から入力される位置指令を差分近似により微分した指令速度である。ｎ０は減速開始時のサンプリングを表す。減速開始時の速度ｖｄ（＝ｖｍａｘ）はｄｖｄの４倍の大きさとなっている。したがって、式（１４）より、
【００６８】
ｋｄｅｎｄ  ＝  ｎ０  ＋  ４  −  １  ＝  ｎ０  ＋  ３      ・・・（１５）
となり、減速が終了する時点のサンプリングはｎ０＋３となる。
トルクＦＦ信号補正部１０では、（ｋｄｅｎｄ  −  ｓ）以降のトルクフィードフォワードを強制的に０にする処理を行う。
【００６９】
図３（ｂ）で、斜線部は、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓを０サンプリングとした時に、余分となるトルクフィードフォワード信号である。この場合、
【００７０】
ｋｄｅｎｄ−ｓ  ＝  ｎ０＋３−０  ＝  ｎ０＋３
以降のトルクフィードフォワード信号（斜線部のトルク）を強制的に０にする。
【００７１】
図３（ｃ）で、斜線部は、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓを１サンプリングとした時に、余分となるトルクフィードフォワード信号である。この場合、
【００７２】
ｋｄｅｎｄ−ｓ  ＝  ｎ０＋３−１  ＝  ｎ０＋２
以降のトルクフィードフォワード信号（斜線部のトルク）を強制的に０にする。
【００７３】
図３（ｄ）で、斜線部は、トルク指令払い出しから出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れｓを１．５サンプリングとした時に、余分となるトルクフィードフォワード信号である。この場合、
【００７４】
ｋｄｅｎｄ−ｓ  ＝  ｎ０＋３−１．５  ＝  ｎ０＋１．５
以降のトルクフィードフォワード信号（斜線部のトルク）を強制的に０にする。
【００７５】
このように、計算結果が整数でない場合は、ｋｎｅｎｄ−ｓの小数点以下を切り捨てたサンプリング時はトルクフィードフォワードの値信号を１／αとし、小数点以下を切り上げたサンプリング時はトルクフィードフォワード信号を０とするというようにすることで対応できる。
【００７６】
すなわち、図３（ｄ）のような場合は、１．５の小数点以下を切り捨てたｎ０＋１回目のサンプリングにはトルクフィードフォワード信号を１／αとして、小数点以下を切り上げたｎ０＋２のサンプリング時はトルクフィードフォワード信号を０とするようにすればよい。
【００７７】
また、αは調整により求めても良いが、小数点以下の値の逆数にしても良い。すなわち、図３（ｄ）のような場合は、小数点以下の値は０．５なのでα＝１／０．５＝２とすればよい。
以上が本発明の実施例１の説明である。
【００７８】
図３は、本発明の効果を示すシミュレーション結果である。図３（ａ）は本発明を用いない従来の動作時の速度（上段）と偏差（下段）の波形である。図から分かるように、加速終了時と、減速終了時にもトルクフィードフォワード信号による過補償によって、大きな偏差が発生している。
図３（ｂ）は本発明を使用した際の速度（上段）と偏差（下段）の波形である。図から分かるように、加速終了時と、減速終了時にトルクフィードフォワード信号による過補償が発生しないため、偏差がほとんど０となっているのが分かる。
【００７９】
このように、指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求め、トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降のトルクフィードフォワード信号を強制的に０とするという手順で処理するため、結果としてトルクフィードフォワード信号が過補償とならず、加速終了時および位置決め時の偏差を非常に小さくすることができるのである。
【実施例２】
【００８０】
図２は、本発明の第２の方法を実施するデジタルサーボ制御装置の構成を示すブロック図である。本実施例が実施例１と異なる点は、実施例１では１００のデジタルサーボ制御装置で位置指令に対して位置制御を行っていたのに対し、本実施例では速度指令に対して速度制御を行うところである。基本的な方法は実施例１と同様であるため以下に実施例１と異なる部分のみ説明する。
【００８１】
加速終了時予測部８では、式（１６）に示すように、指令払い出し開始時ｋ０回目のサンプリングの速度の増分値ｄｖａを求め、式（１７）に示すように、上位指令器から受け取ったｖｍａｘとｄｖａから加速終了時のサンプリングｋａｅｎｄを出力する。予め入力される指令が速度指令ｖｒｅｆ（ｋ）であるため、ｄｖａの計算は実施例１と異なる。
【００８２】
ｄｖａ＝｛ｖｒｅｆ（ｋ０）−ｖｒｅｆ（ｋ０−１）｝            ・・・（１６）
【００８３】
ｋａｅｎｄ＝ｋ０  ＋  ｖｍａｘ／ｄｖａ  −  １              ・・・（１７）
【００８４】
減速終了時予測手段９では、式（１８）に示すように、減速開始時ｎ０回目のサンプリグ部の速度の減分値ｄｖｄを求め、式（１９）に示すように、減速開始時の速度ｖｄ（＝ｖｍａｘ）とｄｖｄから減速終了時のサンプリングｋｄｅｎｄを出力する。
【００８５】
ｄｖｄ＝｛ｖｒｅｆ（ｎ０）−ｖｒｅｆ（ｎ０−１）｝            ・・・（１８）
【００８６】
ｋｄｅｎｄ＝ｎ０  ＋  ｖｄ／ｄｖｄ  −  １                  ・・・（１９）
トルクＦＦ信号補正部１０では、サンプリング（ｋａｅｎｄ  −  ｓ）および（ｋｄｎｄ  −  ｓ）以降のトルクフィードフォワード信号を強制的に０にする処理を行う。
以上が、本発明の実施例２の説明である。この構成の場合も実施例１と同様の効果が得られる。
【産業上の利用可能性】
【００８７】
指令払い出し開始１回目のサンプリング時の速度指令の増分値ｄｖａと最大速度ｖｍａｘを用いて、加速が終了するサンプリングｋａｅｎｄを計算で求め、トルク指令払い出しから、出力されたトルク指令によって制御対象が動作した検出値を取得するまでの遅れをｓサンプリングとした時、（ｋａｅｎｄ  ―  ｓ）サンプリング以降の加速時のトルクフィードフォワード信号を強制的に０とするという手順で処理するため、トルクフィードフォワード信号の過補償を防げ、加速終了時および位置決め時の偏差を非常に小さくすることができるため、単軸のメカだけでなくロボットや工作機などのような多軸のメカの位置決めという用途にも適用できる。
【図面の簡単な説明】
【００８８】
【図１】本発明の方法を適用するデジタルサーボ制御装置の構成を示すブロック図
【図２】本発明の方法を適用するデジタルサーボ制御装置の構成２を示すブロック図
【図３】本発明の作用を説明する図
【図４】本発明をシミュレーションした結果の図
【図５】従来のデジタルサーボ制御装置の構成を示すブロック図
【符号の説明】
【００８９】
１  制御対象
２  検出器
３  位置制御部
４  速度制御部
５  電流制御部
６  速度ＦＦ作成部
７  トルクＦＦ作成部
８  加速終了時予測部
９  減速終了時予測部
１０  トルクＦＦ信号補正部
３０  位置ループ
３２  速度ループ積分要素
３４  速度フィードバックゲイン
３６  電流ループ
３８  サーボモータ電気部
４０  サーボモータ機械部
４２  積分
４４  位置のフィードフォワード項
４６  速度のフィードフォワード項
１００  デジタルサーボ制御装置
【図１】
0004496545.tif 000002
【図２】
0004496545.tif 000003
【図３】
0004496545.tif 000004
【図４】
0004496545.tif 000005
【図５】
0004496545.tif 000006