JP5865116B2

JP5865116B2 - 位置制御装置

Info

Publication number: JP5865116B2
Application number: JP2012038324A
Authority: JP
Inventors: 匡宮路
Original assignee: Okuma Corp
Current assignee: Okuma Corp
Priority date: 2012-02-24
Filing date: 2012-02-24
Publication date: 2016-02-17
Anticipated expiration: 2032-02-24
Also published as: JP2013174999A

Description

本発明は、工作機械等における送り軸（テーブル、サドルあるいは主軸頭等の被駆動体）の位置制御装置、特に位置指令値に従い被駆動体位置をフルクローズド制御する位置制御装置の改良に関する。

工作機械の可動部にリニアスケールを取り付けて被駆動体位置を検出し、これを位置指令値と比較してフルクローズド制御を行う位置制御装置において、位置誤差を小さくするために以下の試みがなされている。

過渡応答時の位置誤差を小さくするためには、速度ループと位置ループの各ゲインを高く設定することで、可動部の摺動抵抗の変化や切削負荷等の予測困難な負荷変動外乱に対して被駆動体を高精度に制御することが可能となる。

図５に一般的なフルクローズド制御システムのブロック図を示す。リニアスケール１１で検出した被駆動体１２の位置検出値Ｐｌを位置フィードバック値として位置指令Ｐｃとの偏差Ｐｄｉｆを減算器２が算出し、速度指令演算器３が前記位置偏差Ｐｄｉｆに基づき比例ゲインＫｐを乗算し速度指令Ｖｃ出力する。一方、モータ１０に取り付けられた位置検出器９の位置検出値Ｐｍを微分器１４が微分し、モータの速度検出値Ｖｍを出力する。前記速度指令Ｖｃとモータ速度検出値Ｖｍの偏差を減算器４により求め、速度偏差として出力する。この速度偏差と速度ループ比例ゲインＰｖと速度ループ積分ゲインＩｖに基づきトルク指令演算器（速度ループ比例ゲイン）５とトルク指令演算器（速度ループ積分ゲイン）６がそれぞれ速度偏差比例成分と速度偏差積分成分を出力し、加算器７が速度偏差比例成分と速度偏差積分成分を加算しトルク指令Ｔｃを出力する。図５中の記号８は、トルク指令をフィルタリングする各種のフィルタ部と電流制御部を示す。電流制御部はトルク指令Ｔｃに従い、モータ１０を制御し、ボールネジ１３を介して被駆動体１２を駆動する。

ここで、図５中の記号８で記した各種フィルタ部・電流制御部の伝達特性が１であるとし、モータと被駆動体がバネ係数Ｋｂのバネで連結されているとする。更に、モータ、被駆動体のモータ軸換算イナーシャがそれぞれＪｍ、Ｊｌ、粘性摩擦係数がそれぞれＤｍ、Ｄｌ、被駆動体に作用する摺動抵抗などのトルク外乱がＴｄであるとしてモデル化すると、トルク指令Ｔｃから被駆動体の位置検出値Ｐｌまでの伝達特性は、図６に示すブロック図で表すことができる。

また、速度指令Ｖｃからモータの速度検出値Ｖｍまでの伝達特性が１、即ち、図５における速度フィードバック制御系の応答帯域が十分に高く、被駆動体からの反力トルクＴｒの影響を抑制可能な系に構成できたとすると、図５のブロック図は図７に示すブロック図で表され、制御系全体の伝達関数は次の式１で表される。式１においてＳはラプラス演算子を示す。なお、ここでは、簡単のため、トルク外乱Ｔｄ＝０としている。

Ｐｌ（Ｓ）／Ｐｃ（Ｓ）
＝Ｋｐ・Ｋｂ／（Ｊｌ・Ｓ^３＋Ｄｌ・Ｓ^２＋Ｋｂ・Ｓ＋Ｋｐ・Ｋｂ）・・・式１

上式において、Ｋｐ＜＜（Ｋｂ／Ｊｌ）^１／２と設定した場合、制御系全体のゲイン線図は図１０に示す様な特性となる。

近年、各種のフィルタ技術と制振制御および速度ループの高速化によって高い位置・速度ループゲインの設定が可能になった。しかし、経年変化による駆動系部品の磨耗・部品のゆるみ、連続動作時の温度上昇によるボールネジの伸びを原因としたボールネジのテンション低下などにより、送り軸機構部の剛性が低下する場合がある。その場合、機械共振周波数（Ｋｂ／Ｊｌ）^１／２が低下し、式１における制御系全体のゲイン線図は図１１に示す様な特性となる。即ち、高く設定された位置ループゲインにより機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕が低下し、場合によっては被駆動体が低周波で振動してしまう課題が生じていた。また、大型マシニングセンタ等で想定以上の重量ワークを被駆動体に積載した場合においても、機械共振周波数（Ｋｂ／Ｊｌ）^１／２が低下するため、同様の低周波振動が発生する。この課題に対する従来技術を次に説明する。

図８は、低周波振動の抑制を目的とした従来技術を示す制御ブロック図である。減算器１５は前記被駆動体位置検出値Ｐｌと前記モータ位置検出値Ｐｍの差であるたわみ量Ｐｓを算出する。たわみ量検出器３２は、たわみ量Ｐｓに応じた係数Ｋを算出し、ゲイン初期値Ｋｐ０を乗算して速度指令演算器３で使用するゲインＫｐを決定する。ここで、係数Ｋは、たわみ量Ｐｓが大きくなるに従いその値が小さくなるように構成されている。この時、ゲインＫｐはＫｐ＝Ｋｐ０・Ｋであるから、たわみ量Ｐｓが大きくなるに従い、ゲインＫｐは小さくなる。ここで、送り軸機構部の剛性とたわみ量Ｐｓの関係に着目すると、送り軸機構部の剛性は被駆動体とモータを連結するバネ係数Ｋｂのバネで表現されており、剛性が低下、即ち、バネによる結合が弱まれば被駆動体位置検出値Ｐｌとモータ位置検出値Ｐｍの差であるたわみ量Ｐｓは大きくなる。即ち、たわみ量検出器３２は、送り軸機構部の剛性が低下すると前記たわみ量Ｐｓが大きくなることに着目し、速度指令演算器３のゲインＫｐを小さくしている。

送り軸機構部の剛性が低下した状態では、制御系全体のゲイン線図は図１１に示す様な特性であったが、送り軸機構部の剛性の低下にあわせて速度指令演算器３のゲインＫｐを小さくしたことにより、図７のブロック図における制御系全体のゲイン線図は図１２の実線で示す様な特性となる。結果、機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕を大きくし、図５の従来例で発生した低周波の振動に対する課題を解決している。

ここで、図７における速度指令演算器３のゲインＫｐの最適値について説明する。機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕を大きくし、制御系の安定性を確保するためには、ゲインＫｐの値を小さくすれば良い。しかし、過度にゲイン余裕を確保することは制御系の応答性を低下させ、負荷変動外乱を抑制する効果や過渡応答時の位置誤差を小さくする効果が損なわれることとなる。したがって、機械共振周波数（Ｋｂ／Ｊｌ）^１／２の低下、即ち、送り軸機構部の剛性の低下度合いにあわせて、速度指令演算器３のゲインＫｐを小さくすることが好ましい。図８の従来例では、図５の従来例で発生した低周波の振動を解決すると同時に、送り軸機構部の剛性の低下にあわせて速度指令演算器３のゲインＫｐを変更することで、過度にゲイン余裕が確保され、制御系の応答性が著しく低下することを回避している。

図９は、共振周波数の存在する制御対象に対して、制御パラメータの調整を行う他の従来技術を示す図である。速度指令演算器３の位置ループゲインＫｐ、トルク指令演算器５、６の速度ループ比例ゲインＰｖと速度ループ積分ゲインＩｖを含む制御パラメータのセットを予め定めておき、ゲイン設定値が低いものから順にセット番号が割り振られている。セット番号を指定すると制御パラメータとして各ゲインの値が設定され、決められた動作を行ったときに速度検出値に振動が発生していないかチェックを行う。このとき、振動の発生を検出した場合には発振フラグをセットし、セット番号と関連付けて管理する。この動作をゲイン設定値が低いセットから順に行うことによって、発振限界を見極め、結果として駆動系が振動すること無く良好な制御応答を示す制御パラメータを選択することを可能としている。

特開２００７−２１９６８９号公報特開２００４−００７９５５号公報

図８に示した従来技術では、送り軸機構部の剛性の低下にあわせて速度指令演算器３のゲインＫｐを小さくすることで、機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕を確保し、低周波振動を抑制している。ここで、送り軸機構部の剛性の低下はたわみ量Ｐｓの増加により検出される。しかしながら、たわみ量Ｐｓが増加する現象は、送り軸機構部の剛性が低下した場合や被駆動体の積載重量が増加した場合に限らず、被駆動体に作用する摺動抵抗などのトルク外乱Ｔｄが増加した場合にも発生する。被駆動体に作用するトルク外乱Ｔｄが増加したことにより、たわみ量Ｐｓが増加した場合、機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕に変動は無いため、速度指令演算器３のゲインＫｐを小さくする必要は無い。逆に、この状況下で速度指令演算器３のゲインＫｐを小さくすることは、過度にゲイン余裕を確保することとなり、結果として制御系の応答性を低下させてしまう。それどころか、被駆動体に作用する摺動抵抗が軽くなる、即ちトルク外乱Ｔｄが小さくなることでたわみ量Ｐｓが減少した場合、機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕に変動は無いにも関らず、速度指令演算器３のゲインＫｐを大きくすると低周波の振動が発生してしまう。

トルク外乱Ｔｄの影響について詳しく説明する。被駆動体に作用するトルク外乱Ｔｄ≠０とすると被駆動体位置検出値Ｐｌはモータ位置検出値Ｐｍを用いて、下記式２で表される。式２においてＳはラプラス演算子を示す。

Ｐｌ（Ｓ）＝｛Ｋｂ／（Ｊｌ・Ｓ^２＋Ｄｌ・Ｓ＋Ｋｂ）｝・Ｐｍ（Ｓ）
−｛１／（Ｊｌ・Ｓ^２＋Ｄｌ・Ｓ＋Ｋｂ）｝・Ｔｄ（Ｓ）・・・式２

式２においてＴｄ＝０、即ち被駆動体に作用する摺動抵抗などのトルク外乱Ｔｄを無視した場合、式２は図７に示すブロック図のバネ系モデル３０と等価である。即ち、トルク外乱の有無に関わらず、機械共振周波数は（Ｋｂ／Ｊｌ）^１／２であり、送り軸機構部の剛性、及び被駆動体の積載重量によって決まる。

しかし、被駆動体位置検出値Ｐｌは被駆動体に作用するトルク外乱Ｔｄの影響を受ける。式２においてＳ＝０とし、極低周波での駆動を想定した場合、式２は下記式３のように展開される。式３においてＳはラプラス演算子を示す。

Ｐｌ（Ｓ）＝Ｐｍ（Ｓ）−｛１／Ｋｂ｝・Ｔｄ（Ｓ）・・・式３

つまり、送り軸機構部の剛性低下、即ちバネ係数Ｋｂが小さくなる、あるいはトルク外乱Ｔｄが大きくなることによって、被駆動体位置検出値Ｐｌとモータ位置検出値Ｐｍの差であるたわみ量Ｐｓの大きさは増大する。

更に、摺動抵抗などのトルク外乱Ｔｄの大きさは被駆動体の速度によって変化する。例えば、被駆動体が滑りガイド機構により支持・駆動される場合、高速移動時は油膜面上を滑るように移動するため摺動抵抗が小さくなる。一方、低速移動時は油膜の抵抗を受けながら進むため摺動抵抗が大きくなる。この他にも、摺動抵抗の大きさは温度や被駆動体の位置など種々の要因により変動するため、正確に把握することは困難である。即ち、被駆動体に摺動抵抗などのトルク外乱Ｔｄが作用し、変動する駆動系においては、たわみ量Ｐｓから送り軸機構部の剛性低下を検出することは困難であり、速度指令演算器３のゲインＫｐを適切に定めることができない。

一方、図９に示した従来技術では、決められた動作を行ったときに速度検出値に振動が発生していないかチェックを行うことで発振限界を見極め、良好な制御応答を示す制御パラメータのセットを決定している。この場合、被駆動体に摺動抵抗などのトルク外乱Ｔｄが作用・変動しても、機械共振を直接検出しているため、送り軸機構部の剛性にあった制御パラメータを定めることができる。しかし、経年変化による送り軸機構部の剛性低下にあわせて制御パラメータを最適化するためには、定期的に調整動作を行い、発振フラグを設定し直す必要がある。また、セット番号、各制御パラメータの値、発振フラグを関連付けて管理、記憶する必要があり、多くのメモリを必要とする。

本発明が解決しようとする課題は、摺動抵抗など被駆動体にトルク外乱が作用する駆動系において、その大きさが変動することにより、たわみ量の大きさも変動し、送り軸機構部の剛性低下が検出できなくなることである。また、送り軸機構部の剛性低下の度合いにあわせて、速度指令演算器のゲイン設定値などの制御パラメータの値を適切な値に調節することができなくなり、結果として、過度にゲイン余裕が確保され制御系の応答性が低下してしまったり、反対にゲイン余裕が少なくなり被駆動体に低周波振動が発生したりすることである。更に、制御パラメータを最適な状態に維持するために、定期的に調整動作を行うことを必要とし、調整動作の結果を管理、記憶するために、多くのメモリを必要とすることである。そして、本発明の目的は、送り軸機構部の剛性低下や被駆動体の積載重量の増加による機械共振周波数の低下を検出し、共振特性にあわせて速度指令演算器のゲイン設定値などの制御パラメータの値を最適化することで、被駆動体の低周波振動を防止するとともに制御系の追従性の低下を最小限に止める位置制御装置を提供することである。また、制御パラメータの最適化には特定の調整動作や多くの記憶メモリを必要とせず、通常の機械駆動動作の中で最適化を行う位置制御装置を提供することである。

本発明は、上記課題に対してなされたものであり、モータ位置検出器と、モータにより駆動される被駆動体の被駆動体位置検出器と、を含み、被駆動体の位置をフルクローズド制御する被駆動体の位置制御装置であって、前記被駆動体位置検出器からの位置検出値から求まる位置フィードバック値と上位装置から入力される位置指令値との偏差を比例増幅して速度指令値を出力する速度指令演算器と、前記モータ位置検出器からの位置検出値を微分して速度フィードバック値を算出する微分器と、前記速度フィードバック値と前記速度指令値との偏差を比例積分増幅してトルク指令値を出力するトルク指令演算器と、前記トルク指令値に応じて前記モータを駆動する手段と、前記位置指令値と前記被駆動体位置検出器からの位置検出値に基づいて被駆動体の振動の有無を判断し、被駆動体の振動が検出された場合に前記速度指令演算器のゲイン設定値を小さくする経年変化補正器と、を備え、経年変化により機械共振周波数が低下しても低周波振動の発生を抑制する、ことを特徴とする。

他の本発明は、モータ位置検出器と、モータにより駆動される被駆動体の被駆動体位置検出器と、を含み、被駆動体の位置をフルクローズド制御する被駆動体の位置制御装置であって、前記被駆動体位置検出器からの位置検出値から求まる位置フィードバック値と上位装置から入力される位置指令値との偏差を比例増幅して速度指令値を出力する速度指令演算器と、前記モータ位置検出器からの位置検出値を微分してモータ速度検出値を算出する微分器と、前記被駆動体位置検出器からの位置検出値を微分して被駆動体速度検出値を算出する微分器と、前記モータ速度検出値と前記被駆動体速度検出値の差を入力とする高域遮断フィルタの出力と前記モータ速度検出値を加算して速度フィードバック値を算出する加算器と、前記速度フィードバック値と前記速度指令値との偏差を比例積分増幅してトルク指令値を出力するトルク指令演算器と、前記トルク指令値に応じて前記モータを駆動する手段と、前記位置指令値と前記被駆動体位置検出器からの位置検出値を入力とし、被駆動体の振動が検出された場合に前記トルク指令演算器および前記速度指令演算器の少なくとも一方のゲイン設定値を小さくする経年変化補正器と、を備え、経年変化により機械共振周波数が低下しても低周波振動の発生を抑制する、ことを特徴とする。

好適な態様では、前記被駆動体位置検出器からの位置検出値を、前記位置フィードバック値とする。他の好適な態様では、さらに、前記モータ位置検出器からの位置検出値と前記被駆動体位置検出器からの位置検出値の差であるたわみ量を算出する減算器と、前記たわみ量を入力とする高域遮断フィルタの出力と前記モータ位置検出器からの位置検出値を加算して前記位置フィードバック値を算出する加算器と、を備える。

他の好適な態様では、前記経年変化補正器は、前記位置指令値を２階微分して指令加速度を出力する２次微分器と、前記指令加速度の大きさが閾値以下である場合に駆動系が加減速状態に無いと判断し振動検出開始信号を出力する比較器と、前記位置指令値と前記被駆動体位置検出器からの位置検出値との差分から被駆動体の位置誤差を算出する減算器と、前記振動検出開始信号の出力時に前記被駆動体の位置誤差信号に含まれる振動の振動周波数を算出し出力する振動検出器と、前記振動周波数からゲイン換算初期値を算出するゲイン換算初期値設定器と、前記振動検出器が振動を検出している間、前記ゲイン換算初期値に対し予め設定されたゲイン低減率分を繰り返し減算してゲイン換算値を算出する減算器と、前記ゲイン換算値をリミット処理し前記トルク指令演算器および前記速度指令演算器の少なくとも一方のゲイン設定値に乗算するゲイン出力切替器と、を備え、被駆動体の振動が検出された場合に前記トルク指令演算器および前記速度指令演算器の少なくとも一方のゲイン設定値を小さくする。

本発明による位置制御装置によれば、摺動抵抗など被駆動体に作用するトルク外乱の大きさが変動する送り軸駆動系においても、経年変化補正器が機構部の剛性低下に起因した振動を検出し、速度指令演算器、トルク指令演算器のゲイン設定値を小さくする。その結果、送り軸機構部の剛性が低下しても低周波の振動を発生させることなく被駆動体を動作させることができるとともに制御系の追従性の低下を最小限に止めることができる。また、駆動条件の判別、振動周波数の特定、ゲイン設定値の変更を自律的に行うため、特定の調整動作を必要とせず、通常の機械駆動動作の中で最適化を行うことができる。

本発明の実施例を示すブロック図である。本発明の経年変化補正器の実施例を示すブロック図である。本発明の他の実施形態を示すブロック図である。本発明の他の実施形態を示すブロック図である。従来技術を示すブロック図である。制御対象を二慣性系モデルで表現したときのブロック図である。従来技術を示すブロック図である。従来技術を示すブロック図である。従来技術を示すブロック図である。機械共振周波数が低下する前のゲイン線図例である。機械共振周波数が低下したときのゲイン線図例である。速度指令演算器のゲインを小さくしたときのゲイン線図例である。

本発明の実施例について説明する。従来例と同一要素には同一符号を付しており説明は省略する。本発明の制御ブロック図を図１に示す。経年変化補正器４０は位置指令Ｐｃと被駆動体位置検出値Ｐｌを入力とし、駆動系が非加減速状態にあるときの被駆動体の振動を検出する。更に、被駆動体の振動が検出された場合には、速度指令演算器３のゲインＫｐを可変する。

具体的に、速度指令演算器３のゲインＫｐを可変する実施例を図２に示す。位置指令値Ｐｃを２次微分器４１に入力し、２階微分することで指令加速度Ａｃを算出する。算出された指令加速度Ａｃは比較器４３により予め設定された指令加速度閾値Ａｃｒｅｆと比較され、指令加速度Ａｃが指令加速度閾値Ａｃｒｅｆ以下の場合には、駆動系が加減速状態に無い（定常状態にある）と判断し、振動検出開始信号を振動検出器４８に出力する。

一方、振動検出器４８は、振動検出開始信号の他に位置指令値Ｐｃと被駆動体位置検出値Ｐｌとの差分により定義される被駆動体の位置誤差信号を入力に持ち、振動検出開始信号が出力されている間の被駆動体の位置誤差信号に含まれる振動の振動周波数ｆｐを算出し出力する。この時、振動周波数ｆｐの検出範囲は予め定数設定されたｆｓｔからｆｅｎの範囲に限定される。更に、被駆動体の位置誤差信号に含まれる振動の大きさ（振幅）が予め定数設定されたＳＰｒｅｆの値よりも大きい場合にのみ振動が存在していると見なし、振動周波数ｆｐとして出力する。

ゲイン換算初期値設定器４９は、振動検出器４８から出力された振動周波数ｆｐを入力とし、以下の方法でゲイン換算初期値Ｋｓを算出する。

速度指令Ｖｃからモータの速度検出値Ｖｍまでの伝達特性が１、即ち、図１における速度フィードバック制御系の応答帯域が十分に高く、被駆動体からの反力トルクＴｒの影響を抑制可能な系に構成できたとすると、制御系全体の伝達関数は式１で表される。

機械共振周波数（Ｋｂ／Ｊｌ）^１／２における低周波振動を抑制するためには、上記周波数におけるゲイン余裕を確保する必要があり、少なくともＫｐ≦（Ｋｂ／Ｊｌ）^１／２とする必要がある。機械共振周波数で低周波振動が発生した場合、振動検出器４８は振動周波数ｆｐを出力し、これを角周波数で表現すると２πｆｐとなる。即ち、低周波振動を抑制するためにはＫｐ≦２πｆｐとする必要がある。そこで、ゲイン換算初期値設定器４９は式４のようにゲイン換算初期値Ｋｓを演算し、上記関係を満たしていない場合は両者の比率を、上記関係を満たしている場合は１を出力する。

Ｋｓ＝
２πｆｐ／Ｋｐ（Ｋｐ＞２πｆｐの場合）
１（Ｋｐ≦２πｆｐの場合）
・・・式４

一方、振動検出器４８が振動周波数ｆｐを検出している場合、カウンタ５１がカウントアップされ、ゲイン換算初期値Ｋｓから予め定数設定されたゲイン低減率分ΔＫの減算を行い、ゲイン換算値Ｋを算出する。なお、ゲイン換算値Ｋはその値が０＜Ｋ＜１の範囲となるようにゲイン出力切替器５３でリミット処理される。ゲイン換算値Ｋが算出されると、速度指令演算器３のゲインは元々の設定値Ｋｐに対し、ゲイン換算値Ｋを乗じた値Ｋｐ’＝Ｋ×Ｋｐまで低減される。なお、上記カウントアップ操作は振動検出器４８が振動周波数ｆｐを検出し続けている間継続して行われるため、振動検出器４８にて振動が検出されなくなるまでゲイン換算値Ｋの値が小さくなる。そして、この比率分だけ速度指令演算器３のゲインが小さくなる。言い換えれば、被駆動体の振動を抑制するのに必要な分だけゲイン設定値が小さくされ、必要以上に小さなゲインが設定されることを防ぐことができるため、位置制御系の追従性の低下を最小限に止めることができる。

なお、ゲイン換算初期値設定器４９の出力であるゲイン換算初期値Ｋｓ、及びゲイン出力切替器５３の出力であるゲイン換算値Ｋは、振動検出器４８において異なる振動周波数ｆｐが検知された場合に、その値が更新されるものとし、振動検出器４８が振動を検出していない間は、その値が保持されるものとする。

同様に、カウンタ５１のカウンタ値ｎは、振動検出器４８において異なる振動周波数ｆｐが検知された場合に、カウントクリアされるものとし、振動検出器４８が振動を検出していない間は、その値が保持されるものとする。ただし、前述のように同一の振動周波数ｆｐを検出し続けている間はカウントアップ動作を行うものとする。

また、振動検出器４８は、振動検出開始信号が出力されている間の被駆動体の位置誤差信号に含まれる振動の振動周波数ｆｐを算出することを示したが、その振動検出アルゴリズムの実現にあたっては、工学的に良く知られているＤＦＴ（ＦＦＴ）等を利用することによって実現できる。もしくは被駆動体の位置誤差信号が最大値あるいは最小値、またはその中間値をとる時間の間隔を計測することで振動周期を特定し、その逆数を計算することで振動周波数ｆｐを算出することも可能である。

以上のように、本発明による位置制御装置によれば、図１１のように機械共振周波数（Ｋｂ／Ｊｌ）^１／２が低下した場合に発生する低周波振動に対し、振動が観測されなくなるまでゲインKｐを小さくすることにより、機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕が大きくなるため、図１２の実線で示す様なゲイン特性となる。結果として、制御系を安定化し、低周波振動の発生を抑制することができる。また、ゲインＫｐの値として必要以上に小さな値が設定されることがないため、過度にゲイン余裕が確保されることもなく、制御系の追従性の低下を最小限に止めることができる。更に、経年変化補正器４０は、被駆動体の振動を検出してゲインＫｐを可変するため、摺動抵抗など被駆動体に作用するトルク外乱の大きさが変動する送り軸駆動系においても、適切に機構部の剛性低下を検出し、低周波振動を抑制することができる。また、指令加速度Ａｃによる駆動条件の判別、振動検出器による振動周波数の特定、ゲイン設定値の変更など一連の動作を自律的に行うため、特定の調整動作を必要とせず、通常の機械駆動動作の中で最適化を行うことができる。

次に、他の実施形態について、図２、図３を参照して説明する。図３は位置制御装置の制御ブロックを示した図であり、位置フィードバック値を算出するための構成が異なることを除けば、図１に示したものと同じである。

図３に示した位置制御装置では、被駆動体位置検出値Ｐｌとモータ位置検出値Ｐｍの差であるたわみ量Ｐｓを算出する。更に、たわみ量Ｐｓは伝達特性がＧｐ（Ｓ）で与えられる高域遮断フィルタ１６に入力され、その出力と前記モータ位置検出値Ｐｍを加算して位置フィードバック値Ｐｄを算出する。ここで、高域遮断フィルタ１６は、高域遮断特性を持つフィルタであればよく、例えば移動平均演算を行うＦＩＲフィルタであってもよい。なお、ここでは、簡単のため、高域遮断フィルタ１６の伝達特性が１／（１＋Ｔｐ・Ｓ）で与えられる１次遅れ特性であるとして説明する。

図３において、経年変化補正器４０は図１と同様に、位置指令Ｐｃと被駆動体位置検出値Ｐｌを入力とし、駆動系が非加減速状態にあるときの被駆動体の振動を検出する。被駆動体の振動は経年変化補正器４０の振動検出器４８において、その振動周波数ｆｐが特定され、ゲイン換算初期値設定器４９に入力される。そして、ゲイン換算初期値設定器４９は、以下の方法でゲイン換算初期値Ｋｓを算出する。

速度指令Ｖｃからモータの速度検出値Ｖｍまでの伝達特性が１、即ち、図３における速度フィードバック制御系の応答帯域が十分に高く、被駆動体からの反力トルクＴｒの影響を抑制可能な系に構成できたとすると、制御系全体の伝達関数は式５で表される。式５においてＳはラプラス演算子を示す。

Ｐｌ（Ｓ）／Ｐｃ（Ｓ）
＝｛（１＋Ｔｐ・Ｓ）・Ｋｐ・Ｋｂ｝
／｛Ｔｐ・（Ｓ＋Ｋｐ）・（Ｊｌ・Ｓ^３＋Ｄｌ・Ｓ^２＋Ｋｂ・Ｓ）
＋（Ｊｌ・Ｓ^３＋Ｄｌ・Ｓ^２＋Ｋｂ・Ｓ＋Ｋｐ・Ｋｂ）｝・・・式５

式５において、Ｔｐ＝０とした場合、位置フィードバック値Ｐｄに被駆動体位置検出値Ｐｌを用いた場合を表し、式１と一致する。反対に、Ｔｐ＝∞とした場合、位置フィードバック値Ｐｄにモータ位置検出値Ｐｍを用いた場合を表し、式５は下記式６のように展開される。式６においてＳはラプラス演算子を示す。

Ｐｌ（Ｓ）／Ｐｃ（Ｓ）
＝｛Ｋｐ／（Ｓ＋Ｋｐ）｝・｛Ｋｂ／（Ｊｌ・Ｓ^２＋Ｄｌ・Ｓ＋Ｋｂ）｝・・・式６

即ち、１次遅れ特性の時定数Ｔｐの大きさによって程度は異なるものの、図３における位置制御系は、機械共振周波数（Ｋｂ／Ｊｌ）^１／２における低周波振動を生じる恐れがあり、これを抑制するためには、上記周波数におけるゲイン余裕を確保する必要がある。また、そのためには、図１の構成の場合と同様に、少なくともＫｐ≦（Ｋｂ／Ｊｌ）^１／２とする必要がある。そこで、ゲイン換算初期値設定器４９は図１の場合と同様に、式４を用いてゲイン換算初期値Ｋｓを演算し、ゲイン出力切替器５３によりゲイン換算値Ｋが出力される。これにより、速度指令演算器３のゲインは元々の設定値Ｋｐに対し、ゲイン換算値Ｋを乗じた値Ｋｐ’＝Ｋ×Ｋｐまで低減され、振動検出器４８にて振動が検出されなくなるまで速度指令演算器３のゲインが小さくなる。

以上の説明から明らかなとおり、本実施形態によれば、図１、図２を用いて説明した場合と同様の作用効果が得られる。即ち、本実施形態によれば、機械共振周波数（Ｋｂ／Ｊｌ）^１／２が低下した場合に発生する低周波振動に対し、振動が観測されなくなるまでゲインＫｐを小さくすることにより、機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕が大きくなるため、制御系を安定化し、低周波振動の発生を抑制することができる。また、ゲインＫｐの値として必要以上に小さな値が設定されることがないため、過度にゲイン余裕が確保されることもなく、制御系の追従性の低下を最小限に止めることができる。更に、経年変化補正器４０は、被駆動体の振動を検出してゲインＫｐを可変するため、摺動抵抗など被駆動体に作用するトルク外乱の大きさが変動する送り軸駆動系においても、適切に機構部の剛性低下を検出し、低周波振動を抑制することができる。また、指令加速度Ａｃによる駆動条件の判別、振動検出器による振動周波数の特定、ゲイン設定値の変更など一連の動作を自律的に行うため、特定の調整動作を必要とせず、通常の機械駆動動作の中で最適化を行うことができる。

次に、他の実施形態について、図２、図４を参照して説明する。図４は位置制御装置の制御ブロックを示した図であり、速度フィードバック値を算出するための構成が異なることと、速度指令演算器３のゲインＫｐに加え、トルク指令演算器の比例ゲインＰｖ、積分ゲインＩｖを可変する構成となっていることを除けば、図１に示したものと同じである。

図４に示した位置制御装置では、被駆動体位置検出値Ｐｌを微分器１８で微分して得た被駆動体速度検出値Ｖｌと、前記モータ速度検出値Ｖｍの差を伝達特性がＧｖ（Ｓ）で与えられる高域遮断フィルタ２０に入力し、その出力と前記モータ速度検出値Ｖｍを加算して速度フィードバック値Ｖｄを算出する。ここで、高域遮断フィルタ２０は、高域遮断特性を持つフィルタであればよく、例えば移動平均演算を行うＦＩＲフィルタであってもよい。なお、ここでは、簡単のため、高域遮断フィルタ２０の伝達特性が１／（１＋Ｔｖ・Ｓ）で与えられる１次遅れ特性であるとして説明する。

図４において、経年変化補正器４０は図１と同様に、位置指令Ｐｃと被駆動体位置検出値Ｐｌを入力とし、駆動系が非加減速状態にあるときの被駆動体の振動を検出する。被駆動体の振動は経年変化補正器４０の振動検出器４８において、その振動周波数ｆｐが特定され、ゲイン換算初期値設定器４９に入力される。そして、ゲイン換算初期値設定器４９は、以下の方法でゲイン換算初期値Ｋｓを算出する。

記号８で記した各種フィルタ部・電流制御部の伝達特性が１、かつ図６で示したモータ、被駆動体の粘性摩擦係数Ｄｍ、Ｄｌが無視できるとすると、速度指令Ｖｃから速度フィードバック値Ｖｄまでの伝達関数は式７で表される。式７においてＳはラプラス演算子を示す。

Ｖｄ（Ｓ）／Ｖｃ（Ｓ）
＝｛（Ｊｌ・Ｐｖ・Ｔｖ）・Ｓ^４＋（Ｊｌ・Ｉｖ・Ｔｖ）・Ｓ^３
＋（Ｋｂ・Ｐｖ・Ｔｖ）・Ｓ^２
＋（Ｋｂ・Ｐｖ＋Ｋｂ・Ｉｖ・Ｔｖ）・Ｓ＋（Ｋｂ・Ｉｖ）｝
／｛Ｊｍ・Ｊｌ・Ｔｖ・Ｓ^５＋（Ｊｍ・Ｊｌ＋Ｊｌ・Ｐｖ・Ｔｖ）・Ｓ^４
＋（Ｊｍ・Ｋｂ・Ｔｖ＋Ｊｌ・Ｋｂ・Ｔｖ＋Ｊｌ・Ｉｖ・Ｔｖ）・Ｓ^３
＋（Ｊｍ・Ｋｂ＋Ｊｌ・Ｋｂ＋Ｋｂ・Ｐｖ・Ｔｖ）・Ｓ^２
＋（Ｋｂ・Ｐｖ＋Ｋｂ・Ｉｖ・Ｔｖ）・Ｓ＋（Ｋｂ・Ｉｖ）｝・・・式７

ここで、トルク指令演算器のゲイン設定値を小さくすることにより、機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕が確保され、低周波振動が抑制された状態では、共振特性の影響は表面化せず、剛性が高い場合と同じような振る舞いを見せる。そこで、Ｋｂ＝∞と定め、式７を簡略化すると、下記式８のように展開される。式８においてＳはラプラス演算子を示す。

Ｖｄ（Ｓ）／Ｖｃ（Ｓ）
＝｛Ｐｖ・Ｓ＋Ｉｖ｝／｛（Ｊｍ＋Ｊｌ）・Ｓ２＋Ｐｖ・Ｓ＋Ｉｖ｝・・・式８

即ち、速度制御ループの応答帯域は｛Ｉｖ／（Ｊｍ＋Ｊｌ）｝^１／２で表され、機械共振周波数（Ｋｂ／Ｊｌ）^１／２における低周波振動を抑制するためには、少なくとも｛Ｉｖ／（Ｊｍ＋Ｊｌ）｝^１／２≦（Ｋｂ／Ｊｌ）^１／２とする必要がある。機械共振周波数で低周波振動が発生した場合、振動検出器４８は振動周波数ｆｐを出力し、これを角周波数で表現すると２πｆｐとなる。即ち、低周波振動を抑制するためには｛Ｉｖ／（Ｊｍ＋Ｊｌ）｝^１／２≦２πｆｐとする必要がある。そこで、ゲイン換算初期値設定器４９は式９のようにゲイン換算初期値Ｋｓを演算し、上記関係を満たしていない場合は両者の比率を、上記関係を満たしている場合は１を出力する。

｛Ｉｖ／（Ｊｍ＋Ｊｌ）｝^１／２＞２πｆｐの場合：Ｋｓ＝２πｆｐ／｛Ｉｖ／（Ｊｍ＋Ｊｌ）｝^１／２
｛Ｉｖ／（Ｊｍ＋Ｊｌ）｝^１／２≦２πｆｐの場合：Ｋｓ＝１・・・式９

一方、振動検出器４８が振動周波数ｆｐを検出している場合、カウンタ５１がカウントアップされ、ゲイン換算初期値Ｋｓから予め定数設定されたゲイン低減率分ΔＫの減算を行い、ゲイン換算値Ｋを算出する。なお、ゲイン換算値Ｋはその値が０＜Ｋ＜１の範囲となるようにゲイン出力切替器５３でリミット処理される。ゲイン換算値Ｋが算出されると、トルク指令演算器５の速度ループ比例ゲイン、トルク指令演算器６の速度ループ積分ゲインは元々の設定値Ｐｖ、Ｉｖに対し、ゲイン換算値Ｋを乗じた値Ｐｖ’＝Ｋ×Ｐｖ、Ｉｖ’＝Ｋ×Ｉｖまで低減される。なお、上記カウントアップ操作は振動検出器４８が振動周波数ｆｐを検出し続けている間継続して行われるため、振動検出器４８にて振動が検出されなくなるまでゲイン換算値Ｋの値が小さくなる。そして、この比率分だけトルク指令演算器５、６のゲインが小さくなる。言い換えれば、被駆動体の振動を抑制するのに必要な分だけゲイン設定値が小さくされ、必要以上に小さなゲインが設定されることを防ぐことができるため、位置制御系の追従性の低下を最小限に止めることができる。なお、トルク指令演算器のゲイン設定値を小さくした場合、速度フィードバック系の制御応答帯域が低下するため、速度指令演算器３のゲインＫｐを高い設定値のままにしておくと、位置制御系が不安定となり、振動的な応答を示す場合がある。このような場合、ゲイン換算値Ｋに従い、トルク指令演算器の比例ゲインＰｖ、積分ゲインＩｖを低減するのと同時に、速度指令演算器３のゲインＫｐを低減することで解決できる。

なお、ゲイン換算初期値設定器４９が式９に従い、ゲイン換算初期値Ｋｓを演算するためには、モータ、被駆動体のモータ軸換算イナーシャＪｍ、Ｊｌの和が既知である必要がある。この値を算出する方法として、例えば、比較器４３から振動検出開始信号が出力される直前、即ち、駆動系が加減速状態にあり、振動検出開始信号が出力されていないときのトルク指令Ｔｃと、被駆動体位置検出値Ｐｌあるいはモータ位置検出値Ｐｍを二階微分して得られる加速度検出値の比からモータ、被駆動体のモータ軸換算イナーシャＪｍ、Ｊｌの和を同定することで実現できる。また、本実施形態では、図１の場合と同様に、リニアスケール１１で検出した被駆動体１２の位置検出値Ｐｌを位置フィードバック値として減算器２に入力している。しかし、図３の場合と同様に、たわみ量Ｐｓを算出し、当該たわみ量を伝達特性がＧｐ（Ｓ）で与えられる高域遮断フィルタ１６に入力し、その出力と前記モータ位置検出値Ｐｍを加算した値を位置フィードバック値Ｐｄとして、減算器２に入力してもよい。

以上の説明から明らかなとおり、本実施形態によれば、図１、図２を用いて説明した場合と同様の作用効果が得られる。即ち、本実施形態によれば、機械共振周波数（Ｋｂ／Ｊｌ）^１／２が低下した場合に発生する低周波振動に対し、振動が観測されなくなるまでトルク指令演算器の比例ゲインＰｖ、積分ゲインＩｖ、速度指令演算器の比例ゲインＫｐを小さくすることにより、機械共振周波数（Ｋｂ／Ｊｌ）^１／２におけるゲイン余裕が大きくなるため、制御系を安定化し、低周波振動の発生を抑制することができる。また、ゲイン設定値として必要以上に小さな値が設定されることがないため、過度にゲイン余裕が確保されることもなく、制御系の追従性の低下を最小限に止めることができる。更に、経年変化補正器４０は、被駆動体の振動を検出してトルク指令演算器、速度指令演算器のゲインを可変するため、摺動抵抗など被駆動体に作用するトルク外乱の大きさが変動する送り軸駆動系においても、適切に機構部の剛性低下を検出し、低周波振動を抑制することができる。また、指令加速度Ａｃによる駆動条件の判別、振動検出器による振動周波数の特定、ゲイン設定値の変更など一連の動作を自律的に行うため、特定の調整動作を必要とせず、通常の機械駆動動作の中で最適化を行うことができる。

なお、図１、図３、図４の各構成において、式４、式９を用いてゲイン換算初期値Ｋｓを算出することを示したが、式４、式９は低周波振動を検出した後、いち早くこれを抑制するための初期値を演算するものであり、必ずしも式４、式９に基づきゲイン換算初期値Ｋｓを定める必要はない。例えば、振動検出器４８が検出した振動周波数ｆｐに関わらず、無条件にゲイン換算初期値Ｋｓを１としてもよい。

１位置指令演算器、２，４，１５，１９，２２，２５，２７，４４，５２減算器、３速度指令演算器（位置ループゲイン）、５トルク指令演算器（速度ループ比例ゲイン）、６トルク指令演算器（速度ループ積分ゲイン）、７，１７，２１加算器、８各種フィルタ部・電流制御部、９モータ位置検出器、１０モータ、１１リニアスケール、１２被駆動体、１３ボールネジ、１４，１８微分器、１６，２０高域遮断フィルタ、２３モータモデル、２４，２９積分器、２６バネモデル、２８被駆動体モデル、３０バネ系モデル、３１速度指令からモータ速度の伝達特性、３２たわみ量検出器、４０経年変化補正器、４１２次微分器、４２，４５，４６，４７，５０係数、４３比較器、４８振動検出器、４９ゲイン換算初期値設定器、５１カウンタ、５３ゲイン出力切替器。

Claims

モータ位置検出器と、モータにより駆動される被駆動体の被駆動体位置検出器と、を含み、被駆動体の位置をフルクローズド制御する被駆動体の位置制御装置であって、
前記被駆動体位置検出器からの位置検出値から求まる位置フィードバック値と上位装置から入力される位置指令値との偏差を比例増幅して速度指令値を出力する速度指令演算器と、
前記モータ位置検出器からの位置検出値を微分して速度フィードバック値を算出する微分器と、
前記速度フィードバック値と前記速度指令値との偏差を比例積分増幅してトルク指令値を出力するトルク指令演算器と、
前記トルク指令値に応じて前記モータを駆動する手段と、
前記位置指令値と前記被駆動体位置検出器からの位置検出値に基づいて被駆動体の振動の有無を判断し、被駆動体の振動が検出された場合に前記速度指令演算器のゲイン設定値を小さくする経年変化補正器と、
を備え、
経年変化により機械共振周波数が低下しても低周波振動の発生を抑制する、
ことを特徴とする位置制御装置。
モータ位置検出器と、モータにより駆動される被駆動体の被駆動体位置検出器と、を含み、被駆動体の位置をフルクローズド制御する被駆動体の位置制御装置であって、
前記被駆動体位置検出器からの位置検出値から求まる位置フィードバック値と上位装置から入力される位置指令値との偏差を比例増幅して速度指令値を出力する速度指令演算器と、
前記モータ位置検出器からの位置検出値を微分してモータ速度検出値を算出する微分器と、
前記被駆動体位置検出器からの位置検出値を微分して被駆動体速度検出値を算出する微分器と、
前記モータ速度検出値と前記被駆動体速度検出値の差を入力とする高域遮断フィルタの出力と前記モータ速度検出値を加算して速度フィードバック値を算出する加算器と、
前記速度フィードバック値と前記速度指令値との偏差を比例積分増幅してトルク指令値を出力するトルク指令演算器と、
前記トルク指令値に応じて前記モータを駆動する手段と、
前記位置指令値と前記被駆動体位置検出器からの位置検出値を入力とし、被駆動体の振動が検出された場合に前記トルク指令演算器および前記速度指令演算器の少なくとも一方のゲイン設定値を小さくする経年変化補正器と、
を備え、
経年変化により機械共振周波数が低下しても低周波振動の発生を抑制する、
ことを特徴とする位置制御装置。
請求項１または２に記載の位置制御装置であって、
前記被駆動体位置検出器からの位置検出値を、前記位置フィードバック値とする、ことを特徴とする位置制御装置。
請求項１または２に記載の位置制御装置であって、さらに、
前記モータ位置検出器からの位置検出値と前記被駆動体位置検出器からの位置検出値の差であるたわみ量を算出する減算器と、
前記たわみ量を入力とする高域遮断フィルタの出力と前記モータ位置検出器からの位置検出値を加算して前記位置フィードバック値を算出する加算器と、
を備えることを特徴とする位置制御装置。
請求項１から４のいずれか１項に記載の位置制御装置であって、
前記経年変化補正器は、
前記位置指令値を２階微分して指令加速度を出力する２次微分器と、
前記指令加速度の大きさが閾値以下である場合に駆動系が加減速状態に無いと判断し振動検出開始信号を出力する比較器と、
前記位置指令値と前記被駆動体位置検出器からの位置検出値との差分から被駆動体の位置誤差を算出する減算器と、
前記振動検出開始信号の出力時に前記被駆動体の位置誤差信号に含まれる振動の振動周波数を算出し出力する振動検出器と、
前記振動周波数からゲイン換算初期値を算出するゲイン換算初期値設定器と、
前記振動検出器が振動を検出している間、前記ゲイン換算初期値に対し予め設定されたゲイン低減率分を繰り返し減算してゲイン換算値を算出する減算器と、
前記ゲイン換算値をリミット処理し前記トルク指令演算器および前記速度指令演算器の少なくとも一方のゲイン設定値に乗算するゲイン出力切替器と、
を備え、
被駆動体の振動が検出された場合に前記トルク指令演算器および前記速度指令演算器の少なくとも一方のゲイン設定値を小さくする、
ことを特徴とする位置制御装置。