(57)【要約】【課題】簡易に精度よく位置制御できる位置制御装置を提供する。【解決手段】モータ位置検出器と、モータにより駆動される被駆動体の位置を検出する被駆動体位置検出器と、を含み、被駆動体の位置をフルクローズド制御する被駆動体の位置制御装置は、前記位置指令値と前記被駆動体位置検出値との差分値のピークを検出し、このピークに基づいて前記差分値に含まれる振動の周期と振幅を検出する振動周期振幅検出器６８と、前記位置指令値に基づいて、前記被駆動体が加減速状態でないことを判定する加減速状態判定部６７と、加減速状態でない場合に前記振動周期振幅検出器により検出された振動の周期が周期閾値以上かつ振幅が振幅閾値以上の場合に得られる周期を、前記定常的な振動の周期として出力する定常振動検出器６９と、前記定常振動検出器から出力された前記振動の周期に基づいて、前記制御パラメータを変更する制御パラメータ変更部７０と、を備える。【選択図】図４
2014153842.tif 000002
【特許請求の範囲】
【請求項１】
モータ位置検出器と、モータにより駆動される被駆動体の位置を検出する被駆動体位置検出器と、を含み、被駆動体の位置をフルクローズド制御する被駆動体の位置制御装置であって、
上位装置から入力される位置指令値と位置フィードバック値との偏差を比例増幅して速度指令値を出力する速度指令演算器と、
前記速度指令値と速度フィードバック値との偏差を、比例積分増幅して、トルク指令値を出力するトルク指令演算器と、
前記トルク指令値に応じて、前記モータを駆動する駆動部と、
前記位置指令値と、前記被駆動体位置検出器で検出される被駆動体位置検出値に基づいて、前記被駆動体の定常的な振動の有無を検出し、前記定常的な振動が発生している場合に前記定常的な振動の周期を算出し、前記周期に応じて制御パラメータを変更する経年補正器を、備え、
前記経年補正器は、
前記位置指令値と前記被駆動体位置検出値との差分値の極値タイミングおよび極値を検出し、検出された極値タイミングおよび極値に基づいて前記差分値に含まれる振動の周期および振幅を検出する振動周期振幅検出器と、
前記位置指令値に基づいて、前記被駆動体が加減速状態でないことを判定する加減速状態判定部と、
加減速状態でない場合に前記振動周期振幅検出器により検出された振動の周期が周期閾値以上かつ振幅が振幅閾値以上の場合に得られる周期を、前記定常的な振動の周期として出力する定常振動検出器と、
前記定常振動検出器から出力された前記振動の周期に基づいて、前記制御パラメータを変更する制御パラメータ変更部と、
を備えることを特徴とする位置制御装置。
【請求項２】
請求項１に記載の位置制御装置であって、
前記周期閾値は、前記振動周期振幅検出器により検出された振動の周期の最大値である振動周期最大値に、規定の閾値を減算した値である、ことを特徴とする位置制御装置。
【請求項３】
請求項１または２に記載の位置制御装置であって、
前記定常振動検出器は、加減速状態でなく、かつ、前記振動の周期が周期閾値以上の場合に、前記振動周期振幅検出器により検出された振動の振幅を平滑化し、当該平滑化された振幅が振幅閾値以上か否かを判断する、ことを特徴とする位置制御装置。
【請求項４】
請求項１から３のいずれか１項に記載の位置制御装置であって、
前記位置フィードバック値は、前記被駆動体位置検出器で検出される被駆動体位置検出値であり、
前記速度フィードバック値は、前記モータ位置検出器からの位置検出値を微分することで算出されるモータ速度検出値であり、
前記制御パラメータ変更部は、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【請求項５】
請求項１から３のいずれか１項に記載の位置制御装置であって、
前記位置フィードバック値は、前記モータ位置検出器からの位置検出値と前記被駆動体位置検出値との差を入力とする１次遅れ回路の出力と、前記モータ位置検出器からの位置検出値と、を加算した値であり、
前記速度フィードバック値は、前記モータ位置検出器からの位置検出値を微分することで算出されるモータ速度検出値であり、
前記制御パラメータ変更部は、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【請求項６】
請求項１から３のいずれか１項に記載の位置制御装置であって、
前記位置フィードバック値は、前記モータ位置検出器からの位置検出値と前記被駆動体位置検出値との差を入力とする１次遅れ回路の出力と、前記モータ位置検出器からの位置検出値と、を加算した値であり、
前記速度フィードバック値は、前記モータ位置検出器からの位置検出値を微分することで算出されるモータ速度検出値であり、
前記制御パラメータ変更部は、前記定常的な振動の周期を初期値とし、前記定常的な振動が検出される度に予め設定された時定数増加分を繰り返し加算して得られる値を、前記１次遅れ回路の時定数として順次更新する時定数変更部を備える、
ことを特徴とする位置制御装置。
【請求項７】
請求項６に記載の位置制御装置であって、
前記制御パラメータ変更部は、さらに、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記時定数が予め規定された基準値以上か否かを判定する比較器と、
前記時定数が予め規定された基準値以上の場合に、前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【請求項８】
請求項１から３のいずれか1項に記載の位置制御装置であって、さらに、
前記モータ位置検出器からの位置検出値を微分してモータ速度検出値を算出する微分器と、
前記被駆動体位置検出値を微分して被駆動体速度検出値を算出する微分器と、
を備え、
前記位置フィードバック値は、前記被駆動体位置検出器で検出される被駆動体位置検出値であり、
前記速度フィードバック値は、前記モータ速度検出値と前記被駆動体速度検出値との差を入力とする１次遅れ回路の出力と、前記モータ速度検出値と、を加算した値であり、
前記制御パラメータ変更部は、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【請求項９】
請求項１から３のいずれか1項に記載の位置制御装置であって、さらに、
前記モータ位置検出器からの位置検出値を微分してモータ速度検出値を算出する微分器と、
前記被駆動体位置検出値を微分して被駆動体速度検出値を算出する微分器と、
を備え、
前記位置フィードバック値は、前記被駆動体位置検出器で検出される被駆動体位置検出値であり、
前記速度フィードバック値は、前記モータ速度検出値と前記被駆動体速度検出値との差を入力とする１次遅れ回路の出力と、前記モータ速度検出値と、を加算した値であり、
前記制御パラメータ変更部は、前記定常的な振動の周期を初期値とし、前記定常的な振動が検出される度に予め設定された時定数増加分を繰り返し加算して得られる値を、前記１次遅れ回路の時定数として順次更新する時定数変更部を備える、
ことを特徴とする位置制御装置。
【請求項１０】
請求項９に記載の位置制御装置であって、
前記制御パラメータ変更部は、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記時定数が予め規定された基準値以上か否かを判定する比較器と、
前記時定数が予め規定された基準値以上の場合に、前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値、前記トルク指令演算器のゲイン設定値の少なくとも一つに乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、工作機械等における送り軸（テーブル、サドルあるいは主軸頭等の被駆動体）の位置制御装置、特に位置指令値に従い被駆動体位置をフルクローズド制御する位置制御装置の改良に関する。
【背景技術】
【０００２】
工作機械の可動部にリニアスケールを取り付けて被駆動体位置を検出し、これを位置指令値と比較してフルクローズド制御を行う位置制御装置において、位置誤差を小さくするために以下の試みがなされている。
【０００３】
過渡応答時の位置誤差を小さくするためには、速度ループと位置ループの各ゲインを高く設定することで、可動部の摺動抵抗の変化や切削負荷等の予測困難な負荷変動外乱に対して被駆動体を高精度に制御することが可能となる。
【０００４】
図７に一般的なフルクローズド制御システムのブロック図を示す。被駆動体１２の位置を直接検出するリニアスケール１１で検出した被駆動体１２の位置検出値Ｐｌを位置フィードバック値とし、当該位置検出値Ｐｌと位置指令Ｐｃとの偏差Ｐｄｉｆを減算器２が算出する。速度指令演算器３は、前記位置偏差Ｐｄｉｆに、比例ゲインＫｐを乗算し速度指令Ｖｃを出力する。一方、被駆動体１２を駆動するモータ１０に取り付けられたモータ位置検出器９の位置検出値Ｐｍを微分器１４が微分し、モータの速度検出値Ｖｍを出力する。前記速度指令Ｖｃとモータ速度検出値Ｖｍの偏差を減算器４により求め、速度偏差として出力する。トルク指令演算器５は、速度偏差と速度ループ比例ゲインＰｖを、トルク指令演算器６は、速度偏差の積分値と速度ループ積分ゲインＩｖを、それぞれ乗算して、速度偏差比例成分と速度偏差積分成分を出力する。加算器７は、速度偏差比例成分と速度偏差積分成分を加算しトルク指令Ｔｃを出力する。図７における符号８は、トルク指令をフィルタリングする各種のフィルタ部と電流制御部を示す。電流制御部はトルク指令Ｔｃに従い、モータ１０を制御し、ボールネジ１３を介して被駆動体１２を駆動する駆動部として機能する。
【０００５】
ここで、図７において符号８で記した各種フィルタ部、電流制御部の伝達特性が１であるとし、モータと被駆動体がバネ係数Ｋｂのバネで連結されているとする。更に、モータ、被駆動体１２のモータ軸換算イナーシャがそれぞれＪｍ，Ｊｌ、粘性摩擦係数がそれぞれＤｍ，Ｄｌであるとしてモデル化すると、トルク指令Ｔｃから被駆動体の位置検出値Ｐｌまでの伝達特性は、図８に示すブロック図で表すことができる。
【０００６】
図８では、モータの速度検出値をＶｍ、ラプラス演算子をＳとしたときに、モータの加速トルクＪｍ・Ｓ・Ｖｍ、モータの粘性摩擦Ｄｍ・Ｖｍ、非駆動体からの反力トルクＴｒの総和がトルク指令Ｔｃに等しいとしてモデル化しており、モータモデル１６にトルク指令Ｔｃと反力トルクＴｒの差を入力することでモータの速度検出値Ｖｍが得られるとして表現している。更に、モータの速度検出値Ｖｍを積分器１７で積分することでモータの位置検出値Ｐｍが得られ、バネ系モデル１８の入力となる。
【０００７】
バネ系モデル１８では、モータの位置検出値Ｐｍと被駆動体の位置検出値Ｐｌの差にバネモデル２０のバネ係数Ｋｂを乗じた値が、被駆動体への伝達トルクとなる。なお、この被駆動体への伝達トルクは、非駆動体からの反力トルクＴｒと等しく、被駆動体の速度検出値をＶｌとしたときに、被駆動体の加速トルクＪｌ・Ｓ・Ｖｌ、被駆動体の粘性摩擦Ｄｌ・Ｖｌの総和に等しいとしてモデル化でき、図８では非駆動体モデル２１に被駆動体への伝達トルクを入力することで被駆動体の速度検出値Ｖｌが得られるとして表現している。更に、被駆動体の速度検出値Ｖｌを積分器２２で積分することで被駆動体の位置検出値Ｐｌを得ることができる。
【０００８】
ここで、図７において、速度指令Ｖｃからモータの速度検出値Ｖｍまでの伝達特性が１、即ち、速度フィードバック制御系の応答帯域が十分に高く、図８における被駆動体からの反力トルクＴｒの影響を抑制可能な系に構成できたとすると、図７のブロック図は図９に示すブロック図で表すことができる。
【０００９】
即ち、速度指令Ｖｃからモータの速度検出値Ｖｍの伝達特性は符号２３のように１に置き換えることができ、更に、図８の構成と同様、モータの速度検出値Ｖｍを積分器１７で積分することでモータの位置検出値Ｐｍを得る。また、このモータの位置検出値Ｐｍはバネ系モデル１８の入力となり、バネ系モデル１８は被駆動体の位置検出値Ｐｌを出力する。このとき、制御系全体の伝達関数は次の式１で表される。式１においてＳはラプラス演算子を示す。
Ｐｌ（Ｓ）／Ｐｃ（Ｓ）
＝Ｋｐ・Ｋｂ／（Ｊｌ・Ｓ３＋Ｄｌ・Ｓ２＋Ｋｂ・Ｓ＋Ｋｐ・Ｋｂ）・・・式１
【００１０】
上式において、Ｋｐ＜＜（Ｋｂ／Ｊｌ）１／２と設定した場合、制御系全体のゲイン線図は図１３に示す様な特性となる。
【００１１】
近年、各種のフィルタ技術と制振制御および速度ループの高速化によって高い位置・速度ループゲインの設定が可能になった。しかし、経年変化による駆動系部品の磨耗・部品のゆるみ、連続動作時の温度上昇によるボールネジの伸びを原因としたボールネジのテンション低下などにより、送り軸機構部の剛性が低下する場合がある。その場合、機械共振周波数（Ｋｂ／Ｊｌ）１／２が低下し、式１における制御系全体のゲイン線図は図１４に示す様な特性となる。即ち、高く設定された位置ループゲインにより機械共振周波数（Ｋｂ／Ｊｌ）１／２におけるゲイン余裕が低下し、場合によっては被駆動体が低周波で振動してしまう課題が生じていた。また、大型マシニングセンタ等で想定以上の重量ワークを被駆動体に積載した場合においても、機械共振周波数（Ｋｂ／Ｊｌ）１／２が低下するため、同様の低周波振動が発生する。この課題に対する従来技術を次に説明する。
【００１２】
図１０、図１１は、低周波振動の抑制を目的とした従来技術を示す制御ブロック図である。図７と同一要素には同一符号を付しており、説明を省略する。また、各図に記載している経年変化補正器４０については、詳細を後述する。
【００１３】
図１０において、位置検出値演算器２７は、被駆動体位置検出値Ｐｌとモータ位置検出値Ｐｍより、下記式２で表される位置を位置フィードバック値Ｐｄとして出力する。なお、式２でＴｐは１次遅れ回路２５の時定数を示し、Ｓはラプラス演算子を示す。
Ｐｄ＝Ｐｍ＋（Ｐｌ−Ｐｍ）／（１＋Ｔｐ・Ｓ）                ・・・式２
【００１４】
式２において、１／（１＋Ｔｐ・Ｓ）は１次遅れ回路を示し、式２の第２項の演算は図１０中の１次遅れ回路２５で行われる。
【００１５】
図１１において、速度検出値演算器３２は、被駆動体位置検出値Ｐｌを微分器２８で微分して得た被駆動体速度検出値Ｖｌと、モータ速度検出値Ｖｍより、下記式３で表される速度を速度フィードバック値Ｖｄとして出力する。式３でＴｖは１次遅れ回路３０の時定数を示し、Ｓはラプラス演算子を示す。
Ｖｄ＝Ｖｍ＋（Ｖｌ−Ｖｍ）／（１＋Ｔｖ・Ｓ）                ・・・式３
【００１６】
式３において、１／（１＋Ｔｖ・Ｓ）は１次遅れ回路を示し、式３の第２項の演算は図１１中の１次遅れ回路３０で行われる。
【００１７】
次に、経年変化補正器４０について説明する。経年変化補正器４０は、位置指令Ｐｃと被駆動体位置検出値Ｐｌを入力とし、駆動系が非加減速状態にあるときの被駆動体の振動を検出する。被駆動体の振動が検出された場合に、１次遅れ回路２５，３０の時定数Ｔｐ，Ｔｖを大きく、あるいは、速度指令演算器３のゲインＫｐ、トルク指令演算器５の速度ループ比例ゲインＰｖ、トルク指令演算器６の速度ループ積分ゲインＩｖを小さくする。
【００１８】
式２、式３において、Ｔｐ＞＞（Ｋｂ／Ｍ）１／２、Ｔｖ＞＞（Ｋｂ／Ｍ）１／２の条件のもと、Ｔｐ，Ｔｖの値を大きくした場合、図１０、図１１のブロック図における制御系全体のゲイン線図は図１５の点線で示す様な特性となり、機械共振周波数（Ｋｂ／Ｍ）１／２におけるゲイン余裕が大きくなる。さらに、送り軸機構部の剛性が低下した場合は、図１３の実線で示す様な特性となり、図７の従来例で発生した低周波の振動に対する課題を解決している。
【００１９】
また、送り軸機構部の剛性が低下した状態で、速度指令演算器３のゲインＫｐを小さくした場合、図１０、図１１のブロック図における制御系全体のゲイン線図は図１６の実線で示す様な特性となる。結果、機械共振周波数（Ｋｂ／Ｊｌ）１／２におけるゲイン余裕を大きくし、図７の従来例で発生した低周波の振動に対する課題を解決している。
【００２０】
ここで、経年変化補正器４０の構成について説明する。図１２は経年変化補正器４０の構成を示す制御ブロック図である。位置指令値Ｐｃを２次微分器４１に入力し、２階微分することで指令加速度Ａｃを算出する。算出された指令加速度Ａｃは比較器４３により予め設定された指令加速度閾値Ａｃｒｅｆと比較され、指令加速度Ａｃが指令加速度閾値Ａｃｒｅｆ以下の場合には、駆動系が加減速状態に無い（定常状態にある）と判断し、振動検出開始信号を振動検出器４８に出力する。
【００２１】
一方、振動検出器４８は、振動検出開始信号の他に位置指令値Ｐｃと被駆動体位置検出値Ｐｌとの差分により定義される被駆動体の位置誤差信号を入力に持ち、振動検出開始信号が出力されている間の被駆動体の位置誤差信号に含まれる振動の振動周波数ｆｐをＤＦＴ（ＦＦＴ）などの周波数解析法を利用して算出する。この時、振動周波数ｆｐの検出範囲は予め定数設定されたｆｓｔからｆｅｎの範囲に限定され、被駆動体の位置誤差信号に含まれる振動の大きさ（振幅）が予め定数設定されたＳＰｒｅｆの値よりも大きい場合にのみ振動が存在していると見なし、振動周波数ｆｐを出力する。
【００２２】
除算器４９は、振動周波数ｆｐが出力された場合に逆数である振動周期を算出し、時定数初期値Ｔ０を出力する。算出された時定数初期値Ｔ０は、１次遅れ回路２５または３０で使用する時定数Ｔｐ，Ｔｖの初期値として設定される。時定数Ｔｐ，Ｔｖの値が更新されても振動検出器４８が振動周波数ｆｐを検出し続けている場合、カウンタ５１がカウントアップされ、予め定数設定された時定数増加量分ΔＴの加算が行われ、時定数増加量分ΔＴだけ時定数Ｔｐ、Ｔｖの値が大きくなる。
【００２３】
一方、ゲイン換算初期値設定器５３は、振動検出器４８から出力された振動周波数ｆｐを入力とし、以下の演算を行い、ゲイン換算初期値Ｋｓを算出する。
図１１の構成で｛Ｉｖ／（Ｊｍ＋Ｊｌ）｝１／２＞２πｆｐの場合
Ｋｓ＝２πｆｐ／｛Ｉｖ／（Ｊｍ＋Ｊｌ）｝１／２
図１０の構成で、Ｋｐ＞２πｆｐの場合
Ｋｓ＝２πｆｐ／Ｋｐ
上記以外の場合
Ｋｓ＝１                                      ・・・式４
【００２４】
算出されたゲイン換算初期値Ｋｓは、ゲイン換算値Ｋの初期値として設定される。ゲイン換算値Ｋの値が更新されても振動検出器４８が振動周波数ｆｐを検出し続けている場合、カウンタ５５がカウントアップされ、予め定数設定されたゲイン低減率分ΔＫの減算が行われ、ゲイン低減率分ΔＫだけゲイン換算値Ｋの値が小さくなる。なお、ゲイン換算値Ｋはその値が０＜Ｋ＜１の範囲となるようにゲイン出力切替器５９でリミット処理される。また、ゲイン換算値Ｋが出力されると、図１０の構成の場合は速度指令演算器３のゲインＫｐを、図１１の構成の場合は速度指令演算器３のゲインＫｐ、トルク指令演算器５の速度ループ比例ゲインＰｖ、トルク指令演算器６の速度ループ積分ゲインＩｖを元々の設定値に対し、ゲイン換算値Ｋを乗じた値に低減する。
【００２５】
なお、図１２の従来例では、ゲイン出力切替器５９にて、ゲイン換算値Ｋの更新よりも時定数Ｔｐ，Ｔｖの更新を優先することを可能としている。具体的には、比較器５８にて時定数Ｔｐ，Ｔｖと予め定数設定された許容時定数Ｔｒｅｆとを比較し、比較の結果、時定数Ｔｐ，Ｔｖが許容時定数Ｔｒｅｆを超えた場合にゲイン換算値Ｋの更新を行う。
【００２６】
以上のように、図１２に示した経年変化補正器４０は、１次遅れ回路２５，３０の時定数Ｔｐ，Ｔｖを必要な分だけ大きく、あるいは、速度指令演算器３のゲインＫｐ、トルク指令演算器５の速度ループ比例ゲインＰｖ、トルク指令演算器６の速度ループ積分ゲインＩｖを必要な分だけ小さくすることで、図７の従来例で発生した低周波の振動を解決すると同時に、過度にゲイン余裕が確保され、制御系の応答性が著しく低下することを回避している。
【先行技術文献】
【特許文献】
【００２７】
【特許文献１】特開２０１２−１６８９２６号公報
【発明の概要】
【発明が解決しようとする課題】
【００２８】
図１２に示した従来技術では、被駆動体の位置誤差信号に含まれる振動の振動周波数ｆｐをＤＦＴ（ＦＦＴ）などの周波数解析法を利用して算出している。しかし、この場合、低周波振動の振動周波数を特定する上で、（１）周波数分解能を高くとれず、時定数の算出精度が悪い、（２）メモリの使用量が大きい、（３）データのトレース・解析に時間がかかるなどの課題がある。
【００２９】
例えば、サンプリング周期が１ｍｓのシステムで、１０Ｈｚ相当の低周波振動を検出する場合、サンプリング数を１０２４ポイントとしてＦＦＴ演算を行うことを考えると、周波数分解能は０．９８Ｈｚとなる。結果、１０Ｈｚ前後のスペクトルとしては、９．７７Ｈｚ、１０．７４Ｈｚに対応したスペクトルを得ることができる。しかし、振動周期に換算した場合、それぞれ１０２．４ｍｓ、９３．１ｍｓとなり、９．３ｍｓの差がある。これは、サンプリング周期１ｍｓに比べ、非常に大きい。
【００３０】
特許文献１では、経年変化補正器４０が変更する時定数Ｔｐ，Ｔｖは、１次遅れ回路２５，３０に限らず、高域遮断特性を持つ移動平均（ＦＩＲフィルタ）にも適用できるとし、制御系のサンプリング周期Ｔｓに対し、Ｔｐ／Ｔｓ段、Ｔｖ／Ｔｓ段の移動平均を構成することが記載されている。しかし、時定数Ｔｐ，Ｔｖの値が１０２．４ｍｓであるか９３．１ｍｓであるかによって、該当周波数での遮断特性は大きく異なったものとなる。
【００３１】
なお、時定数の算出精度を高める方法として、サンプリング数を増やすという手段がある。例えば、サンプリング数を８１９２ポイントとすることで、９．８９Ｈｚ、１０．０１Ｈｚに対応したスペクトルを得ることができ、振動周期に換算した場合、それぞれ１０１．１ｍｓ、９９．９ｍｓとなり、サンプリング周期１ｍｓ相当の分解能を持った時定数Ｔｐ，Ｔｖを設定することが可能となる。
【００３２】
ただし、サンプリング数を増やせば、そのデータを格納したり、解析したりするために必要なメモリも増大する。また、時系列データをトレースするに際しても、サンプリング周期×サンプリング数分の連続したデータが必要となるため、トレースに時間を要する他、サンプリング数が増えたことにより、ＦＦＴのバタフライ演算を行う時間も増大する。
【００３３】
一方、被駆動体の位置誤差信号に含まれる振動の振動周波数ｆｐを特定する他の手段として、特許文献１には、被駆動体の位置誤差信号が最大値あるいは最小値をとる時間の間隔から振動周波数ｆｐの逆数である振動周期（時定数初期値Ｔ０）を検出することが記載されている。
【００３４】
しかし、位置誤差信号に含まれる振動が最大値から最小値に遷移する過程、最小値から最大値に遷移する過程は、単調減少、単調増加を繰り返すわけではない。例えば、検出器ノイズ等の影響で頻繁に増加、減少を繰り返し、振動振幅も振動周期ごとに変長する。ここで、位置誤差信号が増加から減少に転じるタイミングを振動の最大値、減少から増加に転じるタイミングを振動の最小値と判断すると、ノイズ等の影響で振動周期の真値よりも遥かに短い周期が検出されてしまう。また、振動を複数周期含む時系列データの中から最大値、最小値を検出すると、変長等の影響で振動周期の真値よりも遥かに長い周期が検出されてしまう。
【００３５】
このように、単純に、時系列データの最大値あるいは最小値をとる時間の間隔から振動周期を特定すると、振動周期を誤検出するという課題がある。
【００３６】
また、送り軸機構部の剛性が低下した状態では、定常的に振動が発生するため、時定数Ｔｐ，Ｔｖ、あるいはゲインＫｐ，Ｐｖ，Ｉｖを更新し、振動を抑制する必要がある。反対に、定常的に振動が発生していない状態では、これらのパラメータを変更する必要はない。しかし、図１２の従来技術では、インパルス上の外乱が瞬時的に加わった場合でも、パラメータが変更されるため、過度にゲイン余裕が確保され、制御系の応答性を低下させるという課題がある。
【００３７】
本発明が解決しようとする課題は、被駆動体の位置誤差信号に含まれる振動の振動周波数ｆｐ（時定数初期値Ｔ０）を正確に特定するために多くのメモリ、時間を必要とすることである。また、時系列データから振動周期（時定数初期値Ｔ０）を検出しようとした場合に、検出器ノイズ等の影響で誤検出することである。更に、インパルス上の外乱が瞬時的に加わった場合に、時定数Ｔｐ，Ｔｖ、あるいはゲインＫｐ，Ｐｖ，Ｉｖが更新され、過度にゲイン余裕が確保され、制御系の応答性を低下させることである。そして、本発明の目的は、多くのメモリ、時間を必要とせず、かつ検出器ノイズ等の影響を受けることなく、正確に被駆動体の低周波振動を検出し、抑制する位置制御装置を提供することである。また、インパルス上の外乱などが加わっても、過度にゲイン余裕を確保することなく、制御系の追従性の低下を最小限に止める位置制御装置を提供することである。
【課題を解決するための手段】
【００３８】
本発明の位置制御装置は、モータ位置検出器と、モータにより駆動される被駆動体の位置を検出する被駆動体位置検出器と、を含み、被駆動体の位置をフルクローズド制御する被駆動体の位置制御装置であって、上位装置から入力される位置指令値と位置フィードバック値との偏差を比例増幅して速度指令値を出力する速度指令演算器と、前記速度指令値と速度フィードバック値との偏差を、比例積分増幅して、トルク指令値を出力するトルク指令演算器と、前記トルク指令値に応じて、前記モータを駆動する駆動部と、前記位置指令値と、前記被駆動体位置検出器で検出される被駆動体位置検出値に基づいて、前記被駆動体の定常的な振動の有無を検出し、前記定常的な振動が発生している場合に前記定常的な振動の周期を算出し、前記周期に応じて制御パラメータを変更する経年補正器を、備え、前記経年補正器は、前記位置指令値と前記被駆動体位置検出値との差分値の極値タイミングおよび極値を検出し、検出された極値タイミングおよび極値に基づいて前記差分値に含まれる振動の周期および振幅を検出する振動周期振幅検出器と、前記位置指令値に基づいて、前記被駆動体が加減速状態でないことを判定する加減速状態判定部と、加減速状態でない場合に前記振動周期振幅検出器により検出された振動の周期が周期閾値以上かつ振幅が振幅閾値以上の場合に得られる周期を、前記定常的な振動の周期として出力する定常振動検出器と、前記定常振動検出器から出力された前記振動の周期に基づいて、前記制御パラメータを変更する制御パラメータ変更部と、を備えることを特徴とする。
【００３９】
好適な態様では、前記周期閾値は、前記振動周期振幅検出器により検出された振動の周期の最大値である振動周期最大値に、規定の閾値を減算した値である。他の好適な態様では、前記定常振動検出器は、加減速状態でなく、かつ、前記振動の周期が周期閾値以上の場合に、前記振動周期振幅検出器により検出された振動の振幅を平滑化し、当該平滑化された振幅が振幅閾値以上か否かを判断する。
【００４０】
他の好適な態様では、前記位置フィードバック値は、前記被駆動体位置検出器で検出される被駆動体位置検出値であり、前記速度フィードバック値は、前記モータ位置検出器からの位置検出値を微分することで算出されるモータ速度検出値であり、前記制御パラメータ変更部は、前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、を備える。
【００４１】
他の好適な態様では、前記位置フィードバック値は、前記モータ位置検出器からの位置検出値と前記被駆動体位置検出値との差を入力とする１次遅れ回路の出力と、前記モータ位置検出器からの位置検出値と、を加算した値であり、前記速度フィードバック値は、前記モータ位置検出器からの位置検出値を微分することで算出されるモータ速度検出値であり、前記制御パラメータ変更部は、前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、を備える。
【００４２】
他の好適な態様では、前記位置フィードバック値は、前記モータ位置検出器からの位置検出値と前記被駆動体位置検出値との差を入力とする１次遅れ回路の出力と、前記モータ位置検出器からの位置検出値と、を加算した値であり、前記速度フィードバック値は、前記モータ位置検出器からの位置検出値を微分することで算出されるモータ速度検出値であり、前記制御パラメータ変更部は、前記定常的な振動の周期を初期値とし、前記定常的な振動が検出される度に予め設定された時定数増加分を繰り返し加算して得られる値を、前記１次遅れ回路の時定数として順次更新する時定数変更部を備える。この場合、前記制御パラメータ変更部は、さらに、前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、前記時定数が予め規定された基準値以上か否かを判定する比較器と、前記時定数が予め規定された基準値以上の場合に、前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、を備えることが望ましい。
【００４３】
他の好適な態様では、さらに、前記モータ位置検出器からの位置検出値を微分してモータ速度検出値を算出する微分器と、前記被駆動体位置検出値を微分して被駆動体速度検出値を算出する微分器と、を備え、前記位置フィードバック値は、前記被駆動体位置検出器で検出される被駆動体位置検出値であり、前記速度フィードバック値は、前記モータ速度検出値と前記被駆動体速度検出値との差を入力とする１次遅れ回路の出力と、前記モータ速度検出値と、を加算した値であり、前記制御パラメータ変更部は、前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、を備える。
【００４４】
他の好適な態様では、さらに、前記モータ位置検出器からの位置検出値を微分してモータ速度検出値を算出する微分器と、前記被駆動体位置検出値を微分して被駆動体速度検出値を算出する微分器と、を備え、前記位置フィードバック値は、前記被駆動体位置検出器で検出される被駆動体位置検出値であり、前記速度フィードバック値は、前記モータ速度検出値と前記被駆動体速度検出値との差を入力とする１次遅れ回路の出力と、前記モータ速度検出値と、を加算した値であり、前記制御パラメータ変更部は、前記定常的な振動の周期を初期値とし、前記定常的な振動が検出される度に予め設定された時定数増加分を繰り返し加算して得られる値を、前記１次遅れ回路の時定数として順次更新する時定数変更部を備える。この場合、前記制御パラメータ変更部は、前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、前記時定数が予め規定された基準値以上か否かを判定する比較器と、前記時定数が予め規定された基準値以上の場合に、前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値、前記トルク指令演算器のゲイン設定値の少なくとも一つに乗算してゲイン値を変更するゲイン出力切替器と、を備えることが望ましい。
【発明の効果】
【００４５】
本発明による位置制御装置によれば、多くのメモリ、時間を必要とせず、かつ検出器ノイズ等の影響を受けることなく、正確に被駆動体の低周波振動を検出し、抑制することができる。また、インパルス上の外乱などが加わっても、過度にゲイン余裕を確保することなく、制御系の追従性の低下を最小限に止めることができる。
【図面の簡単な説明】
【００４６】
【図１】本発明の実施例を示すブロック図である。
【図２】本発明の実施例を示すブロック図である。
【図３】本発明の実施例を示すブロック図である。
【図４】本発明の実施例を示すブロック図である。
【図５】本発明のピーク検出器の実施例を示すブロック図である。
【図６】本発明の有効データ抽出器の実施例を示すブロック図である。
【図７】従来技術を示すブロック図である。
【図８】制御対象を二慣性系モデルで表現したときのブロック図である。
【図９】従来技術を示すブロック図である。
【図１０】従来技術を示すブロック図である。
【図１１】従来技術を示すブロック図である。
【図１２】従来技術を示すブロック図である。
【図１３】機械共振周波数が低下する前のゲイン線図例である。
【図１４】機械共振周波数が低下したときのゲイン線図例である。
【図１５】１次遅れ回路時定数を大きくしたときのゲイン線図例である。
【図１６】１次遅れ回路時定数を大きく、速度指令演算器のゲインを小さくしたときのゲイン線図例である。
【発明を実施するための形態】
【００４７】
本発明の実施例について説明する。従来例と同一要素には同一符号を付しており説明は省略する。本発明の制御ブロック図を図１、図２、図３に示す。
【００４８】
図１は図７の従来例に対し、位置指令Ｐｃと被駆動体位置検出値Ｐｌを入力とする経年変化補正器６０が付加され、速度指令演算器３のゲインＫｐを可変する構成となっている。経年変化補正器６０は駆動系が非加減速状態にあるときの被駆動体の振動を検出し、被駆動体の振動が検出された場合に、速度指令演算器３のゲインＫｐを小さくする。経年変化補正器６０の詳細については後述する。
【００４９】
図２は図１０の従来例に対し、経年変化補正器４０に変わり、経年変化補正器６０が、１次遅れ回路２５の時定数Ｔｐ、速度指令演算器３のゲインＫｐを可変する構成となっている。経年変化補正器６０は駆動系が非加減速状態にあるときの被駆動体の振動を検出し、被駆動体の振動が検出された場合に、１次遅れ回路２５の時定数Ｔｐを大きく、あるいは、速度指令演算器３のゲインＫｐを小さくする。経年変化補正器６０の詳細については後述する。
【００５０】
図３は図１１の従来例に対し、経年変化補正器４０に変わり、経年変化補正器６０が、１次遅れ回路３０の時定数Ｔｖ、速度指令演算器３のゲインＫｐ、トルク指令演算器５の速度ループ比例ゲインＰｖ、トルク指令演算器６の速度ループ積分ゲインＩｖを可変する構成となっている。経年変化補正器６０は駆動系が非加減速状態にあるときの被駆動体の振動を検出し、被駆動体の振動が検出された場合に、１次遅れ回路３０の時定数Ｔｖを大きく、あるいは、速度指令演算器３のゲインＫｐ、トルク指令演算器５の速度ループ比例ゲインＰｖ、トルク指令演算器６の速度ループ積分ゲインＩｖを小さくする。
【００５１】
次に、経年変化補正器６０について説明する。図４は経年変化補正器６０の構成を示した図である。ブロック６７は、駆動系が加減速状態であるか否かを判定する加減速状態判定部である。すなわち、位置指令値Ｐｃを２次微分器４１に入力し、２階微分することで指令加速度Ａｃを算出する。算出された指令加速度Ａｃは比較器４３により予め設定された指令加速度閾値Ａｃｒｅｆと比較され、指令加速度Ａｃが指令加速度閾値Ａｃｒｅｆ以下の場合には、駆動系が加減速状態に無い（定常状態にある）と判断し、振動検出開始信号を有効データ抽出器６３に出力する。
【００５２】
一方、ピーク検出器６１は、位置指令値Ｐｃと被駆動体位置検出値Ｐｌとの差分により定義される被駆動体の位置誤差信号を入力に持ち、被駆動体の位置誤差信号の極大・極小タイミングを検出する。ピーク検出器６１の制御ブロック図を図５に示す。ピーク検出器６１に入力された被駆動体の位置誤差信号は、予め被駆動体の位置誤差信号に含まれる検出器ノイズの大きさの数倍相当が最小分解能となるように設計された丸め処理演算器６１ａに入力される。この丸め処理演算器６１ａにより被駆動体の位置誤差信号に含まれる検出器ノイズの大半は無視される。次に、丸め処理された被駆動体の位置誤差信号を微分器６１ｂで微分し、微分した信号がゼロクロスするタイミングを検出する。このとき、微分した信号の極性が正から負に変わった場合を極大タイミング、負から正に変わった場合を極小タイミングとし、メモリ６２に出力する。
【００５３】
メモリ６２は、ピーク検出器６１が極大タイミングを検出した場合に、被駆動体の位置誤差信号を＋ｐｅａｋ値として記憶し、直前にピーク検出器６１が極小タイミングを検出してからの経過時間を−ｐｅａｋ  →  ＋ｐｅａｋ間隔として記憶する。反対に、ピーク検出器６１が極小タイミングを検出した場合に、被駆動体の位置誤差信号を−ｐｅａｋ値として記憶し、直前にピーク検出器６１が極大タイミングを検出してからの経過時間を＋ｐｅａｋ  →  −ｐｅａｋ間隔として記憶する。
【００５４】
ここで、−ｐｅａｋ  →  ＋ｐｅａｋ間隔、＋ｐｅａｋ  →  −ｐｅａｋ間隔は被駆動体の位置誤差信号に含まれる振動の振動周期の半分に相当する値であり、両者を加算することで振動周期検出値Ｔｄを得ることができる。なお、振動周期検出値Ｔｄは−ｐｅａｋ  →  ＋ｐｅａｋ間隔、＋ｐｅａｋ  →  −ｐｅａｋ間隔のいずれかを２倍した値で代替することも可能である。
【００５５】
また、＋ｐｅａｋ値と−ｐｅａｋ値の差分は被駆動体の位置誤差信号に含まれる振動の振動振幅に相当する値であり、＋ｐｅａｋ値から−ｐｅａｋ値を減算することで振動振幅検出値Ｖｄを得ることができる。なお、被駆動体の位置誤差信号に定常偏差などが介在していない場合、振動振幅検出値Ｖｄは＋ｐｅａｋ値、−ｐｅａｋ値のいずれかを２倍した値で代替することも可能である。
【００５６】
有効データ抽出器６３は、上記のように算出された振動周期検出値Ｔｄ、振動振幅検出値Ｖｄ、及び比較器４３から出力された振動検出開始信号を入力に持ち、振動周期最大値Ｔｍａｘ、及び振動振幅有効値Ｖｏｋを出力する。有効データ抽出器６３の制御ブロック図を図６に示す。有効データ抽出器６３は、入力された振動周期検出値Ｔｄの最大値を検出し、振動周期最大値Ｔｍａｘとして出力する。その一方で、振動周期最大値Ｔｍａｘと振動周期検出値Ｔｄの差が閾値Ｔｅ  ｒｅｆ以下であることを確認、即ち振動周期最大値Ｔｍａｘと振動周期検出値Ｔｄがほぼ等しい状態にあることを確認する。なお、これは、振動周期検出値Ｔｄが、周期閾値［Ｔｍａｘ−Ｔｅ  ｒｅｆ］以上であることを判定するのと等価である。また、両者がほぼ等しい状態にあり、且つ比較器４３から振動検出開始信号が出力されている場合に、データ抽出器６３ｅは、振動振幅検出値Ｖｄを振動振幅有効値Ｖｏｋとして出力する。
【００５７】
前述のように、位置誤差信号に含まれる検出器ノイズの大半はピーク検出器６１の丸め処理演算器６１ａにより無視できる状態となる。ここで、丸め処理を行う際、丸め処理分解能よりも下位の桁を切上げ、または切捨てするが、稀にこの境界値付近でノイズにより、切上げ、切捨てが交互に行われることがある。この場合、ピーク検出器６１は本来抑制したい振動の極大・極小タイミング以外に、ノイズにより生じた極大・極小タイミングを検出する。結果、本来抑制したい振動の振動周期よりも極端に短い周期が振動周期検出値Ｔｄとして検出される。
【００５８】
このように、ノイズにより振動周期を誤検出した場合、振動周期検出値Ｔｄは真値に比べ、極端に短い値となる。また、機械共振周波数が低下し、低周波振動が発生する状況下では、同一周期の振動が継続的に発生するため、振動周期を繰り返し測定してもその値はほぼ同値となり、その最大値もまた、ほぼ同値となる。
【００５９】
この性質を利用し、振動周期検出値Ｔｄの最大値を見ることで、振動周期の真値が測定でき、同時に、振動周期最大値Ｔｍａｘと振動周期検出値Ｔｄがほぼ等しい状態に無い場合は、ノイズの影響を受け、振動周期を誤検出したと判定することができる。
【００６０】
また、駆動系が加減速した状態では、抑制対象とする低周波振動以外に、種々の振動モードが介在するため、正しく振動周期を特定することができない。
【００６１】
そこで、駆動系が加減速状態に無い（定常状態にある）と判断し、比較器４３が振動検出開始信号を出力した場合で、且つ、振動周期最大値Ｔｍａｘと振動周期検出値Ｔｄがほぼ等しく、ノイズの影響を受けていないと判断した場合に、データ抽出器６３ｅは、振動振幅検出値Ｖｄを振動振幅有効値Ｖｏｋとして出力する。
【００６２】
次に、持続振動判定器６５は、有効データ抽出器６３が振動振幅有効値Ｖｏｋを出力したタイミングで動作し、振動振幅有効値Ｖｏｋを平滑化する。平滑化した値が予め設定された振動振幅閾値Ｖｒｅｆを超えた場合に定常的な振動が発生している、すなわち、持続振動発生中と判定する。そして、持続振動発生中と判定した場合、振動周期最大値Ｔｍａｘを、定常的な振動の振幅とし、当該定常的な振動の振幅を時定数初期値Ｔ０として出力する。なお、振動周期最大値Ｔｍａｘに替えて、持続振動発生中と判定した際の振動振幅検出値Ｔｄを、定常的な振動の周期として出力してもよい。
【００６３】
なお、振動振幅有効値Ｖｏｋの平滑化は、例えば、以下の演算を行うことで実現できるが、この手法に限定されるものではない。
Ｙ（ｎ）＝  （１−Ａ）・Ｙ（ｎ−１）＋Ａ・Ｕ（ｎ）              ・・・式５
Ｕ（ｎ）：入力信号（振動振幅有効値Ｖｏｋ）
Ｙ（ｎ）：出力信号
Ｙ（ｎ−１）：出力信号の前回値
Ａ：係数［０＜Ａ＜１］
【００６４】
また、式５のように振動振幅有効値Ｖｏｋを平滑化することにより、インパルス上の外乱が瞬時的に加わった場合でも、振動振幅閾値Ｖｒｅｆを直ちに超えることはなく、送り軸機構部の剛性が低下した場合など、定常的に振動が発生した場合のみ、時定数初期値Ｔ０を出力することが可能となる。結果、インパルス上の外乱が瞬時的に加わったことに対する影響を抑制することができる。なお、インパルス状の外乱が作用しない、あるいは無視できる機械構成の場合、振動振幅有効値Ｖｏｋは、必ずしも、平滑化しなくてもよく、振動振幅有効値Ｖｏｋと振動振幅閾値Ｖｒｅｆとを直接比較してもよい。
【００６５】
持続振動判定器６５が時定数初期値Ｔ０を出力すると、制御パラメータ変更部７０が、当該時定数初期値Ｔ０（定常的な振動の周期）に基づいて各種制御パラメータを変更する。具体的には、図１２の従来例と同様に、時定数初期値Ｔ０を、１次遅れ回路２５または３０で使用する時定数Ｔｐ，Ｔｖの初期値として設定する。時定数Ｔｐ，Ｔｖの値が更新されても持続振動判定器６５が時定数初期値Ｔ０を出力し続けている場合、カウンタ５１がカウントアップされ、予め定数設定された時定数増加量分ΔＴの加算が行われ、時定数増加量分ΔＴだけ時定数Ｔｐ，Ｔｖの値が大きくなる。したがって、カウンタ５１や加算器５２は、定常的な振動の周期に基づいて、１次遅れ回路の時定数を更新する時定数更新部として機能する。
【００６６】
一方、除算器６６は時定数初期値Ｔ０が出力された場合に逆数である振動周波数ｆｐを算出する。算出された振動周波数ｆｐはゲイン換算初期値設定器５３に入力され、式４の演算を行い、ゲイン換算初期値Ｋｓを算出する。
【００６７】
算出されたゲイン換算初期値Ｋｓは、ゲイン換算値Ｋの初期値として設定される。ゲイン換算値Ｋの値が更新されても持続振動判定器６５が時定数初期値Ｔ０を出力し続けている場合、カウンタ５５がカウントアップされ、予め定数設定されたゲイン低減率分ΔＫの減算が行われ、ゲイン低減率分ΔＫだけゲイン換算値Ｋの値が小さくなる。したがって、ゲイン換算初期値設定器５３や、カウンタ５５、減算器５６は、ゲイン換算値を順次出力するゲイン換算値出力部として機能する。なお、ゲイン換算値Ｋはその値が０＜Ｋ＜１の範囲となるようにゲイン出力切替器５９でリミット処理される。また、ゲイン換算値Ｋが出力されると、図１、図２の構成の場合は速度指令演算器３のゲインＫｐを、図３の構成の場合は速度指令演算器３のゲインＫｐ、トルク指令演算器５の速度ループ比例ゲインＰｖ、トルク指令演算器６の速度ループ積分ゲインＩｖを元々の設定値に対し、ゲイン換算値Ｋを乗じた値に低減する。
【００６８】
なお、図１２の従来例と同様に、ゲイン出力切替器５９にて、ゲイン換算値Ｋの更新よりも時定数Ｔｐ，Ｔｖの更新を優先することも可能である。具体的には、比較器５８にて時定数Ｔｐ，Ｔｖと予め定数設定された許容時定数Ｔｒｅｆとを比較し、比較の結果、時定数Ｔｐ，Ｔｖが許容時定数Ｔｒｅｆを超えた場合にゲイン換算値Ｋの更新を行う。
【００６９】
以上のように、本発明による位置制御装置によれば、図１４のように機械共振周波数（Ｋｂ／Ｊｌ）１／２が低下した場合に発生する低周波振動に対し、振動が観測されなくなるまでＴｐ，Ｔｖの値を大きくする、または速度指令演算器３のゲインＫｐ、トルク指令演算器５の速度ループ比例ゲインＰｖ、トルク指令演算器６の速度ループ積分ゲインＩｖを小さくすることにより、図１５、図１６の実線のように、機械共振周波数（Ｋｂ／Ｊｌ）１／２におけるゲイン余裕が大きくなるため、制御系を安定化し、低周波振動の発生を抑制することができる。
【００７０】
その上で、被駆動体の位置誤差信号に含まれる振動の振動周波数ｆｐ（時定数初期値Ｔ０）を特定する際にＤＦＴ（ＦＦＴ）などの周波数解析法を使用しないため、使用するメモリや演算時間を大幅に削減することができる。また、ピーク検出器６１内の丸め処理演算器６１ａ、及び有効データ抽出器６３を設けることで、検出器ノイズ等の影響で時定数初期値Ｔ０を誤検出することなく、正確に振動周期を特定することができる。更に、持続振動判定器６５を設けることで、インパルス上の外乱が瞬時的に加わった場合に、時定数Ｔｐ，Ｔｖ、あるいはゲインＫｐ，Ｐｖ，Ｉｖが更新され、過度にゲイン余裕が確保されることなく、制御系の追従性の低下を最小限に止めることができる。
【符号の説明】
【００７１】
１  上位制御装置、２，４，１５，１９，２４，２９，４４，５６，６３ｂ  減算器、３  速度指令演算器（位置ループゲイン）、５  トルク指令演算器（速度ループ比例ゲイン）、６  トルク指令演算器（速度ループ積分ゲイン）、７，２６，３１，５２  加算器、８  各種フィルタ部，電流制御部、９  モータ位置検出器、１０  モータ、１１  リニアスケール、１２  被駆動体、１３  ボールネジ、１４，２８  微分器、１６  モータモデル、１７，２２  積分器、１８  バネ系モデル、２０  バネモデル、２１  被駆動体モデル、２３  速度指令からモータ速度の伝達特性、２５，３０  一次遅れ回路（高域遮断フィルタ）、２７  位置検出値演算器、３２  速度検出値演算器、４０，６０  経年変化補正器、４１  ２次微分器、４２，４５，４６，４７，５０，５４，５７，６３ｃ，６４  係数、４３，５８，６３ｄ  比較器、４８  振動検出器、４９，６６  除算器、５１，５５  カウンタ、５３  ゲイン換算初期値設定器、５９  ゲイン出力切替器、６１  ピーク検出器、６１ａ  丸め処理演算器、６１ｂ  微分器、６１ｃ  極値検出器、６２  メモリ、６３  有効データ抽出器、６３ａ  最大値検出器、６３ｅ  データ抽出器、６５  持続振動判定器、６７  加減速状態判定部、６８  振動周期振幅検出器、６９  定常振動検出器、７０  制御パラメータ変更部。
【図１】
2014153842.tif 000003
【図２】
2014153842.tif 000004
【図３】
2014153842.tif 000005
【図４】
2014153842.tif 000006
【図５】
2014153842.tif 000007
【図６】
2014153842.tif 000008
【図７】
2014153842.tif 000009
【図８】
2014153842.tif 000010
【図９】
2014153842.tif 000011
【図１０】
2014153842.tif 000012
【図１１】
2014153842.tif 000013
【図１２】
2014153842.tif 000014
【図１３】
2014153842.tif 000015
【図１４】
2014153842.tif 000016
【図１５】
2014153842.tif 000017
【図１６】
2014153842.tif 000018
【手続補正書】
【提出日】平成26年1月9日(2014.1.9)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
モータ位置検出器と、モータにより駆動される被駆動体の位置を検出する被駆動体位置検出器と、を含み、被駆動体の位置をフルクローズド制御する被駆動体の位置制御装置であって、
上位装置から入力される位置指令値と位置フィードバック値との偏差を比例増幅して速度指令値を出力する速度指令演算器と、
前記速度指令値と速度フィードバック値との偏差を、比例積分増幅して、トルク指令値を出力するトルク指令演算器と、
前記トルク指令値に応じて、前記モータを駆動する駆動部と、
前記位置指令値と、前記被駆動体位置検出器で検出される被駆動体位置検出値に基づいて、前記被駆動体の定常的な振動の有無を検出し、前記定常的な振動が発生している場合に前記定常的な振動の周期を算出し、前記周期に応じて制御パラメータを変更する経年変化補正器を、備え、
前記経年変化補正器は、
前記位置指令値と前記被駆動体位置検出値との差分値の極値タイミングおよび極値を検出し、検出された極値タイミングおよび極値に基づいて前記差分値に含まれる振動の周期および振幅を検出する振動周期振幅検出器と、
前記位置指令値に基づいて、前記被駆動体が加減速状態でないことを判定する加減速状態判定部と、
加減速状態でない場合に前記振動周期振幅検出器により検出された振動の周期が周期閾値以上かつ振幅が振幅閾値以上の場合に得られる周期を、前記定常的な振動の周期として出力する定常振動検出器と、
前記定常振動検出器から出力された前記振動の周期に基づいて、前記制御パラメータを変更する制御パラメータ変更部と、
を備えることを特徴とする位置制御装置。
【請求項２】
請求項１に記載の位置制御装置であって、
前記周期閾値は、前記振動周期振幅検出器により検出された振動の周期の最大値である振動周期最大値に、規定の閾値を減算した値である、ことを特徴とする位置制御装置。
【請求項３】
請求項１または２に記載の位置制御装置であって、
前記定常振動検出器は、加減速状態でなく、かつ、前記振動の周期が周期閾値以上の場合に、前記振動周期振幅検出器により検出された振動の振幅を平滑化し、当該平滑化された振幅が振幅閾値以上か否かを判断する、ことを特徴とする位置制御装置。
【請求項４】
請求項１から３のいずれか１項に記載の位置制御装置であって、
前記位置フィードバック値は、前記被駆動体位置検出器で検出される被駆動体位置検出値であり、
前記速度フィードバック値は、前記モータ位置検出器からの位置検出値を微分することで算出されるモータ速度検出値であり、
前記制御パラメータ変更部は、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【請求項５】
請求項１から３のいずれか１項に記載の位置制御装置であって、
前記位置フィードバック値は、前記モータ位置検出器からの位置検出値と前記被駆動体位置検出値との差を入力とする１次遅れ回路の出力と、前記モータ位置検出器からの位置検出値と、を加算した値であり、
前記速度フィードバック値は、前記モータ位置検出器からの位置検出値を微分することで算出されるモータ速度検出値であり、
前記制御パラメータ変更部は、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【請求項６】
請求項１から３のいずれか１項に記載の位置制御装置であって、
前記位置フィードバック値は、前記モータ位置検出器からの位置検出値と前記被駆動体位置検出値との差を入力とする１次遅れ回路の出力と、前記モータ位置検出器からの位置検出値と、を加算した値であり、
前記速度フィードバック値は、前記モータ位置検出器からの位置検出値を微分することで算出されるモータ速度検出値であり、
前記制御パラメータ変更部は、前記定常的な振動の周期を初期値とし、前記定常的な振動が検出される度に予め設定された時定数増加分を繰り返し加算して得られる値を、前記１次遅れ回路の時定数として順次更新する時定数変更部を備える、
ことを特徴とする位置制御装置。
【請求項７】
請求項６に記載の位置制御装置であって、
前記制御パラメータ変更部は、さらに、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記時定数が予め規定された基準値以上か否かを判定する比較器と、
前記時定数が予め規定された基準値以上の場合に、前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【請求項８】
請求項１から３のいずれか1項に記載の位置制御装置であって、さらに、
前記モータ位置検出器からの位置検出値を微分してモータ速度検出値を算出する微分器と、
前記被駆動体位置検出値を微分して被駆動体速度検出値を算出する微分器と、
を備え、
前記位置フィードバック値は、前記被駆動体位置検出器で検出される被駆動体位置検出値であり、
前記速度フィードバック値は、前記モータ速度検出値と前記被駆動体速度検出値との差を入力とする１次遅れ回路の出力と、前記モータ速度検出値と、を加算した値であり、
前記制御パラメータ変更部は、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値に乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【請求項９】
請求項１から３のいずれか1項に記載の位置制御装置であって、さらに、
前記モータ位置検出器からの位置検出値を微分してモータ速度検出値を算出する微分器と、
前記被駆動体位置検出値を微分して被駆動体速度検出値を算出する微分器と、
を備え、
前記位置フィードバック値は、前記被駆動体位置検出器で検出される被駆動体位置検出値であり、
前記速度フィードバック値は、前記モータ速度検出値と前記被駆動体速度検出値との差を入力とする１次遅れ回路の出力と、前記モータ速度検出値と、を加算した値であり、
前記制御パラメータ変更部は、前記定常的な振動の周期を初期値とし、前記定常的な振動が検出される度に予め設定された時定数増加分を繰り返し加算して得られる値を、前記１次遅れ回路の時定数として順次更新する時定数変更部を備える、
ことを特徴とする位置制御装置。
【請求項１０】
請求項９に記載の位置制御装置であって、
前記制御パラメータ変更部は、
前記定常的な振動の周期から前記定常的な振動の周波数を算出する除算器と、
前記定常的な振動の周波数から算出されるゲイン換算初期値を初期値とし、前記定常的な振動が検出される度に予め設定されたゲイン低減率分を繰り返し減算して得られるゲイン換算値を順次出力するゲイン換算値出力部と、
前記時定数が予め規定された基準値以上か否かを判定する比較器と、
前記時定数が予め規定された基準値以上の場合に、前記ゲイン換算値をリミット処理し、前記ゲイン換算値を、前記速度指令演算器のゲイン設定値、前記トルク指令演算器のゲイン設定値の少なくとも一つに乗算してゲイン値を変更するゲイン出力切替器と、
を備えることを特徴とする位置制御装置。
【手続補正２】
【補正対象書類名】明細書
【補正対象項目名】００３８
【補正方法】変更
【補正の内容】
【００３８】
本発明の位置制御装置は、モータ位置検出器と、モータにより駆動される被駆動体の位置を検出する被駆動体位置検出器と、を含み、被駆動体の位置をフルクローズド制御する被駆動体の位置制御装置であって、上位装置から入力される位置指令値と位置フィードバック値との偏差を比例増幅して速度指令値を出力する速度指令演算器と、前記速度指令値と速度フィードバック値との偏差を、比例積分増幅して、トルク指令値を出力するトルク指令演算器と、前記トルク指令値に応じて、前記モータを駆動する駆動部と、前記位置指令値と、前記被駆動体位置検出器で検出される被駆動体位置検出値に基づいて、前記被駆動体の定常的な振動の有無を検出し、前記定常的な振動が発生している場合に前記定常的な振動の周期を算出し、前記周期に応じて制御パラメータを変更する経年変化補正器を、備え、前記経年変化補正器は、前記位置指令値と前記被駆動体位置検出値との差分値の極値タイミングおよび極値を検出し、検出された極値タイミングおよび極値に基づいて前記差分値に含まれる振動の周期および振幅を検出する振動周期振幅検出器と、前記位置指令値に基づいて、前記被駆動体が加減速状態でないことを判定する加減速状態判定部と、加減速状態でない場合に前記振動周期振幅検出器により検出された振動の周期が周期閾値以上かつ振幅が振幅閾値以上の場合に得られる周期を、前記定常的な振動の周期として出力する定常振動検出器と、前記定常振動検出器から出力された前記振動の周期に基づいて、前記制御パラメータを変更する制御パラメータ変更部と、を備えることを特徴とする。