JP6568035B2 - サーボモータ制御装置、サーボモータ制御方法、及びサーボモータ制御用プログラム - Google Patents

Description

本発明は、サーボモータの動力により駆動する被駆動体の位置を補正する機能を有するサーボモータ制御装置、サーボモータ制御方法、及びサーボモータ制御用プログラムに関する。

従来より、テーブルの上に非加工物（ワーク）を搭載し、テーブルを、サーボモータで連結機構を介して移動させるサーボモータ制御装置がある。テーブル及びワークは被駆動体となる。連結機構は、モータサーボに連結されたカップリングと、カップリングに固定されるボールねじとを有する。ボールねじはナットに螺合される。
このようなサーボモータ制御装置において、サーボモータの動力により駆動する被駆動体（移動体ともいう）の位置を補正する機能を有するサーボモータ制御装置がある。

例えば特許文献１には、連結機構の連結部において被駆動体に作用する駆動力を推定し、推定された駆動力に基づき、位置指令値を補正することの記載がある。 特許文献２には、サーボモータから移動体までの距離と、トルク指令値とからボールねじの伸縮量を計算し、その伸縮量からボールねじに螺着された移動体の位置補正量を計算し、その位置補正量によって位置指令値を補正することの記載がある。 また、特許文献３には、サーボ制御装置は、ボールねじの、サーボモータから遠位側において作用する張力と、ボールねじを両端において支持する１対の固定部の間の距離と、サーボモータの近位側に設けられる固定部から移動体までの距離と、サーボモータに付与されるトルク指令に基づいて、ボールねじの伸縮量を計算し、算出されたボールねじの伸縮量に基づいて、送り軸の位置補正量を計算することの記載がある。

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

本発明者らは、位置指令値を補正する場合に、停止、低速動作時において、推定される駆動力に反応して機械動作と関係のない補正が位置指令値に加わり、補正量の振れが生ずることを見出した。
本発明は、工作機械や産業機械において、さらに精度の高い被駆動体の位置制御が可能なサーボモータ制御装置、サーボモータ制御方法、及びサーボモータ制御用プログラムを提供することを目的とする。

（１） 本発明に係るサーボモータ制御装置は、
サーボモータと、
前記サーボモータにより駆動される被駆動体と、
前記サーボモータと前記被駆動体とを連結し、前記サーボモータの動力を前記被駆動体へ伝える連結機構と、
前記被駆動体の位置指令値を生成する位置指令生成部と、
前記位置指令値を用いて前記サーボモータを制御するモータ制御部と、
前記連結機構との連結部において前記被駆動体に作用する駆動力を推定する力推定部と、
前記力推定部で推定した第１の力推定値と、固定値の第２の力推定値とを切り換える切り換え部と、
前記切り換え部により切り替えられた前記第１又は第２の力推定値に基づき、前記位置指令生成部により生成された前記位置指令値を補正するため補正量を生成する補正量生成部と、
を具備するサーボモータ制御装置である。

（２）上記（１）のサーボモータ制御装置において、
前記補正量生成部は、前記切り換え部により切り替えられた前記第１又は第２の力推定値と第１の係数との積と、前記切り換え部により切り替えられた前記第１又は第２の力推定値と前記サーボモータから前記連結部までの距離と第２の係数との積と、の和を前記補正量とすることが望ましい。
（３）上記（１）又は（２）のサーボ制御装置において、前記第２の力推定値は、推定する前記被駆動体に作用する駆動力を、前記位置指令生成部で作成された位置指令の方向に対応する符号のあらかじめ定めた一定値であることが望ましい。
（４）上記（１）から（３）のいずれかのサーボ制御装置において、前記切り換え部は、前記位置指令生成部が作成した前記位置指令値の指令速度又は指令加速度が所望の値以下のときに、前記第２の力推定値を選択することが望ましい。
（５）上記（１）から（３）のいずれかのサーボ制御装置において、前記切り換え部は、早送り動作時に、前記第２の力推定値を選択することが望ましい。
（６）上記（１）から（５）のいずれかのサーボ制御装置において、前記切り替え部により前記第２の力推定値から前記第１の力推定値に切り替えるときに、前記補正量生成部が生成した前記補正量にフィルタをかけることが望ましい。
（７）上記（１）から（５）のいずれかのサーボ制御装置において、前記モータ制御部は、速度指令作成部とトルク指令作成部とを有し、
前記トルク指令作成部は少なくとも速度偏差を積算する積分器を有し、
前記切り替え部により前記第２の力推定値から前記第１の力推定値に切り替えるときに、前記積分器の書き換えを行うことが望ましい。

（８）本発明のサーボモータ制御装置のサーボモータ制御方法は、
サーボモータと、
前記サーボモータにより駆動される被駆動体と、
前記サーボモータと前記被駆動体とを連結し、前記サーボモータの動力を前記被駆動体へ伝える連結機構と、を備えたサーボモータ制御装置のサーボモータ制御方法において、
前記被駆動体の位置指令値を生成し、
前記連結機構との連結部において前記被駆動体に作用する駆動力を推定し、
前記位置指令値の指令速度若しくは指令加速度が所望の値以下の場合又は早送りの場合に、推定した第１の力推定値から、固定値の第２の力推定値に切り換え、
切り替えられた前記第２の力推定値に基づき、生成された前記位置指令値を補正し、
補正された前記位置指令値を用いて前記サーボモータを制御するサーボモータ制御方法である。
（９）上記（８）のサーボモータ制御方法において、前記位置指令値を補正するための位置指令補正値は、前記第２の力推定値と第１の係数との積と、前記第２の力推定値と前記サーボモータから前記連結部までの距離と第２の係数との積と、の和であることが望ましい。
（１０）上記（８）又は（９）のサーボモータ制御方法において、前記第２の力推定値は、推定する前記被駆動体に作用する駆動力を、前記位置指令の方向に対応する符号のあらかじめ定めた一定値であることが望ましい。
（１１）上記（８）から（１０）のいずれかのサーボモータ制御方法において、前記第２の力推定値から前記第１の力推定値に切り替えるときに、前記位置指令値の補正値にフィルタをかけることが望ましい。
（１２）上記（８）から（１０）のいずれかのサーボモータ制御方法において、補正された前記位置指令値を用いて速度指令を作成し、作成された速度指令を用いてトルク指令を作成し、該トルク指令を用いて前記サーボモータを制御し、
前記トルク指令の作成は少なくとも速度偏差を積算する積分器を用いて行い、
前記第２の力推定値から前記第１の力推定値に切り替えるときに、前記積分器の書き換えを行うことが望ましい。

（１３）本発明のサーボモータ制御用プログラムは、
サーボモータと、
前記サーボモータにより駆動される被駆動体と、
前記サーボモータと前記被駆動体とを連結し、前記サーボモータの動力を前記被駆動体へ伝える連結機構と、を備えたサーボモータ制御装置のサーボモータ制御をコンピュータに実行させるサーボモータ制御用プログラムであって、
前記コンピュータに、
前記被駆動体の位置指令値を生成する処理と、
前記連結機構との連結部において前記被駆動体に作用する駆動力を推定する処理と、
前記位置指令値の指令速度若しくは指令加速度が所望の値以下の場合又は早送りの場合に、推定した第１の力推定値から、固定値の第２の力推定値に切り換える処理と、
切り替えられた前記第２の力推定値に基づき、生成された前記位置指令値を補正する処理と、
補正された前記位置指令値を用いて前記サーボモータを制御する処理と、
を実行させるサーボモータ制御用プログラムである。

本発明によれば、停止、低速動作時においても、推定される駆動力に反応して機械動作と関係のない補正が位置指令値に加わって生ずる補正量の振れを抑制し、精度の高い被駆動体の位置制御が可能となる。

前提技術となるサーボモータ制御装置の構成を示すブロック図である。 補正量の振れを説明する図である。 本発明の第１の実施形態となるサーボモータ制御装置の構成を示すブロック図である。 位置指令補正部の一構成例を含むサーボモータ制御装置の構成を示すブロック図である。 モータ制御部の一構成例及びボールねじの長さ（バネ要素の長さ）を求める距離計算部を含むサーボモータ制御装置の構成を示すブロック図である。 速度指令作成部の一構成例を示すブロック図である。 トルク指令作成部の一構成例を示すブロック図である。 図３に示したサーボモータ制御装置の動作を示すフローチャートである。 本発明の第２の実施形態となるサーボモータ制御装置の構成を示すブロック図である。 フィルタと、フィルタの切り替えスイッチとを示す図である。 フィルタの切り替え動作を示すフローチャートである。 本発明の第３の実施形態となるサーボモータ制御装置の構成を示すブロック図である。 トルク指令作成部の構成を示すブロック図である。 積分器の切り替え動作を示すフローチャートである。

以下、本発明の実施形態について図面を用いて説明する。
まず、本発明の実施形態の説明に先立って前提技術となるサーボモータ制御装置について説明する。
図１は前提技術となるサーボモータ制御装置の構成を示すブロック図である。サーボモータ制御装置は、サーボモータ５０で連結機構６０を介してテーブル７０を移動させ、テーブル７０の上に搭載された被加工物（ワーク）を加工する。連結機構６０は、サーボモータ５０に連結されたカップリング６０１と、カップリングに固定されるボールねじ６０２とを有し、ボールねじ６０２にナット６０３が螺合されている。サーボモータ５０の回転駆動によって、ボールねじ６０２に螺着されたナット６０３がボールねじ６０２の軸方向に移動する。

サーボモータ５０の回転角度位置は、サーボモータ５０に関連付けられた、位置検出部となるエンコーダ４０によって検出され、検出された位置（位置検出値）は位置フィードバックとして利用される。なお、エンコーダ４０は回転速度を検出可能であり、検出された速度（速度検出値）は速度フィードバックとして利用可能である。
サーボモータ制御装置は、図示しない上位制御装置や外部入力装置等から入力されるプログラムや命令に従って、サーボモータ５０の位置指令値を作成する位置指令作成部１０と、位置指令作成部１０が作成した位置指令値とエンコーダ４０が検出した位置検出値との差を求める減算器８０と、この差と位置指令補正部３０から出力される補正値とを加算する加算器９０と、この加算値を用いてサーボモータ５０のトルク指令値を作成するモータ制御部２０と、を有する。

サーボモータ５０の駆動時には、連結機構６０とテーブル７０とに駆動力が作用し、これらは弾性変形するが、連結機構６０はテーブル７０と比較して剛性が低いため、全体の弾性変形のうち、連結機構６０の弾性変形が大部分の割合を占める。連結機構６０が弾性変形すると、サーボモータ５０を指令値通りに回転させた場合であっても、テーブル７０の位置に弾性変形量分の誤差が生じる。このため、この誤差を解消するために、連結機構６０の弾性変形量分だけ位置指令値を補正する。連結機構６０の弾性変形量は、テーブル７０（ナット６０３）と連結機構６０との連結部においてテーブル７０に作用する駆動力に比例し、駆動力は連結部において作用する駆動トルクにより表すことができる。
位置指令補正部３０は、補正量生成部３０１と力推定部３０２とを有する。力推定部３０２は、トルク指令値を用いて連結部において駆動体に作用する駆動力（駆動トルク）を推定する。補正量生成部３０１は、力推定部３０２により推定された駆動力に基づき、位置指令生成部１０により生成された位置指令値を補正するための補正量を生成し補正値を出力する。

本発明者らは図１に示した前提技術のサーボモータ制御装置は、停止、低速動作時においても、推定される駆動力に反応して機械動作と関係のない補正が位置指令値に加わり、図２に示すように補正量の振れが生ずる場合があることを見出した。本発明者らは、停止、低速動作時に、力推定部において推定する駆動力（トルク）の更新を停止して、指令方向を考慮してあらかじめ定めた値に固定すれば、補正量の振れを抑制できることを見出した。また、早送り動作中は基本的に、負荷トルクは一定であり、これに基づいて早送り時も負荷トルクを固定値にして補正することが望ましいことを見出した。
以下、補正量の振れを抑制する本発明のサーボモータ制御装置の実施形態について説明する。以下に説明する本実施形態のサーボモータ制御装置が適用される機械は、レーザー加工機、放電加工機、切削加工機等の工作機械であるが、本発明のサーボモータ制御装置はロボット等の産業機械等にも適用可能である。

（第１の実施形態）
図３は本発明の第１の実施形態となるサーボモータ制御装置の構成を示すブロック図である。図３において、図１のサーボモータ制御装置の構成部材と同一構成部材については同一符号を付して説明を省略する。
図３に示すように、位置指令速度判断部１２０は、位置指令作成部１０が作成した位置指令値を受けて、位置指令値の指令速度（位置指令速度）を生成する。また、位置指令速度判断部１２０は、早送りか切削送りかを指示する信号を上位制御装置や外部入力装置等から受ける。位置指令速度判断部１２０は、指令速度（絶対値）が所望の値以下（０を含む）のときに、又は早送りのときに、連結部において駆動体に作用する駆動力（駆動トルク）を、力推定部３０２により推定された駆動力から固定値に切り替えるように、切り替え部となるスイッチ３０３に切り替え信号を送る。一方、位置指令速度判断部１２０は、指令速度（絶対値）が所望の値を超え、且つ切削送りのときに、連結部において駆動体に作用する駆動力（駆動トルク）を、力推定部３０２により推定された駆動力に切り替えるように、スイッチ３０３に切り替え信号を送る。以下の説明では、位置指令値の指令速度を用いてスイッチ３０３により固定値に切り替える場合について説明するが、位置指令値の指令加速度を用いてスイッチ３０３により固定値に切り替えてもよい。位置指令値の指令速度の代わりに位置指令値の指令加速度を用いる場合には、位置指令速度判断部１２０を、位置指令作成部１０が作成した位置指令値を受けて、位置指令値の指令加速度（位置指令加速度）を生成する位置指令加速度判断部に置き換える。位置指令加速度判断部の機能は指令速度を指令加速度に置き換えた点を除いて位置指令速度判断部１２０の機能と同じである。

補正量生成部３０１は、固定値とされた駆動力又は力推定部３０２により推定された駆動力に基づいて補正量を生成し、加算器９０に出力する。図３では力推定部３０２は、モータ制御部２０から出力されるトルク指令値を用いて連結部において駆動体に作用する駆動力（駆動トルク）を推定しているが、駆動力の推定はこれに限定されず、例えば特許文献１（特開２０１４−１０９７８５号公報）に記載されるように、加減速トルク、外乱トルク等をさらに加えて駆動力を推定したり、トルク指令値でなく、モータ電流を検出する電流検出部の出力を用いてモータトルクを算出して駆動力を推定してもよい。
固定値は、位置指令生成部１０で作成された位置指令の方向に対応する符号のあらかじめ定めた一定値であり、基本的に静止時摩擦力に相当する力の値とする。

位置指令速度判断部１２０は、図２に示す補正量の振れを抑制するようにスイッチ３０３を切り替えればよく、指令速度（絶対値）が所望の値以下（０を含む）のときに、さらに補正量生成部３０１からの出力に基づいて、補正量の大きさが増加方向にあるか、減少方向にあるかを判断し、その判断結果によりスイッチ３０３を切り替えてもよい。例えば、補正量の大きさが増加方向にある場合には、スイッチ３０３で固定値に切り替え、補正量の大きさが減少方向にある場合にはスイッチ３０３で力推定部３０２により推定された駆動力に切り替えるようにしてもよい。逆に補正量の大きさが減少方向にある場合には、スイッチ３０３で固定値に切り替え、補正量の大きさが増加方向にある場合にはスイッチ３０３で力推定部３０２により推定された駆動力に切り替えるようにしてもよい。
このように、指令速度（絶対値）が所望の値以下（０を含む）のときに、補正量の大きさが増加方向にあるか減少方向にあるかを判断してスイッチ３０３を切り替えても、図２に示すような補正量の振れを抑制することができる。

以上説明したように、図３に示したサーボモータ制御装置では、位置指令速度が所望の値以下又は早送り動作の場合に、機械の弾性変形量を計算するのに用いる推定した負荷トルクを一定値に固定する。こうすることで、サーボモータの動作に関係しない補正量を印加せず、ロストモーションに対する補正のみを行うことができる。

図４は位置指令補正部の一構成例を含むサーボモータ制御装置の構成を示すブロック図である。図４の位置指令補正部３２の推定負荷トルク計算部３０７は図３の力推定部３０２に対応し、図４のねじれ係数乗算部３０４、ボールねじ長さ乗算部３０６、形状係数乗算部３０５は図３の補正量生成部３０１に対応する。形状係数はボールねじの単位長さあたりの伸縮量を示す。本実施形態では、連結機構（カッブリング、ボールねじ）に発生する回転軸まわりのねじり弾性変形と軸方向の伸縮弾性変形を推定される負荷トルク又は固定値に基づいて算出し、弾性変形に起因するロストモーションを位置指令値に補正する。この例ではボールねじ長さ乗算部３０６の入力側で推定される負荷トルク又は固定値に切り替えているので、補正量にボールねじの長さに対する依存性を待たせることができる。この際、軸方向の弾性変形はサーボモータから被駆動体までの距離に依存し、移動位置の積算値によりこの距離を推定する。
推定負荷トルクをＴ、ねじれ係数をαとすると、連結部のねじれに関する補正量はα・Ｔとなり、推定負荷トルクをＴ、ボールねじの長さをｄ、形状係数をβとするとボールねじの伸縮に関する補正量はｄ・β・Ｔとなる。そして、合計の補正量がα・Ｔ＋ｄ・β・Ｔとなる。

図５はモータ制御部２０の一構成例及びボールねじの長さ（バネ要素の長さ）を求める距離計算部１３０を含むサーボモータ制御装置の構成を示すブロック図である。図４のボールねじ長さ乗算部３０６で乗算されるボールねじの長さ（バネ要素の長さ）は距離計算部１３０により算出される。図４のモータ制御部２０は速度指令作成部２０１と減算部とトルク指令作成部２０２とを有する。
あらかじめ定める固定値は、基本的に機械の停止時にモータにかかる軸上の力を測定して設定する。この設定した力は、外乱（外力）が無い場合は機械の停止時に被駆動体にかかる静摩擦力に相当し、最大静摩擦力以下の値となる。この設定により、停止状態による多少の誤差はあるが、駆動力の推定を停止して固定値としても、機械の停止時にかかる静摩擦力により生じた弾性変形量に相当する補正をかけることができる。また、機械の早送り時においても、被駆動体にかかる力は基本的に反作用としてかかる駆動方向と逆方向の一定の値であり、機械の早送り時においても駆動力の推定を停止し、指令方向に対応したあらかじめ定めた固定値より補正を行う。機械の低速時にも同様な固定値を用いることができる。
停止時（又は低速時）および早送り時いずれにおいても、補正量そのものではなく、推定負荷トルクを固定化することにより、ボールねじの長さに対するボールねじの弾性変形量の変化（ボールねじが長い(被駆動体までの距離が遠い)場合はねじれ量が増える、短い場合はねじれ量が減る）を補正に反映することができる。
補正量の計算は、（補正量）＝｛（形状係数×ボールねじの長さ）＋ねじれ係数｝｝×（推定負荷トルク）であり、推定負荷トルクを固定値に切り替えても、ボールねじの長さに依存した補正がかかることになる。
ボールねじの長さｄはサーボモータから連結機構までのボールねじの長さであり、テーブルの位置により変わる。
負荷トルクが一定又はほぼ一定となる条件である、位置指令値の指令速度若しくは指令加速度が所望の値以下、又は早送りであるときは位置指令速度判断部からの指令により負荷トルクを固定値とし、負荷トルクの値が不明確で推定が必要な条件である、位置指令値の指令速度若しくは指令加速度が所望の値を超え、且つ切削送りのときは位置指令速度判断部からの指令で、従来通り負荷トルクを推定する。これにより、サーボモータの動作に関係しない補正を行う事態を極力回避し、弾性変形によるロストモーションに対する補正のみを行うことができる。

図６は速度指令作成部２０１の一構成例を示すブロック図である。位置指令作成部１０は位置指令値を作成し、位置指令値と位置フィードバックされた検出位置との差を減算器８０で求め、その差に補正量を加算器９０で加え、補正量を加えた差を、微分器２０１１と位置制御ゲイン２０１３とに入力する。加算器２０１４は、微分器２０１１の出力に係数を乗算した係数部２０１２の出力と、位置制御ゲイン２０１３の出力との加算値を速度指令値として出力する。速度指令値は速度フィードバックされた検出速度との差を減算器１００で求める。
図７はトルク指令作成部２０２の一構成例を示すブロック図である。トルク指令作成部２０２は、減算器１００と接続される比例ゲイン２０２３及び積分器２０２１、積分器２０２１と接続される積分ゲイン２０２２、及び比例ゲイン２０２３の出力と積分ゲイン２０２２の出力とを加算し、トルク指令としてサーボモータ５０に出力する加算器２０２４を備えている。積分器２０２１は入力を積分する。積分ゲイン２０２２は積分器２０２１の出力に係数を乗算し、比例ゲイン２０２３は入力に係数を乗算する。なお、積分ゲイン２０２２と積分器２０２１は並び順を変えてもよい。

図８は図３に示したサーボモータ制御装置の動作を示すフローチャートである。ステップＳ１０１において、力推定部３０２で推定負荷トルクを計算する。ステップＳ１０２において、位置指令速度判断部１２０で位置指令値の指令速度が所望の値以下（０を含む）かどうかを判断するとともに、早送りかどうかを判断し、指令速度が所望の値以下（０を含む）、又は早送りの場合（ステップＳ１０２のＹＥＳの場合）は、ステップＳ１０５においてスイッチ３０３で固定値に切り替え、ステップＳ１０４に進む。

一方、ステップＳ１０２において、位置指令値の指令速度が所望の値以下でなく、早送りでない場合、すなわち、指令速度が所望の値を超え且つ切削送りの場合（ステップＳ１０２のＮＯの場合）は、ステップＳ１０３において、スイッチ３０３で、力推定部３０２で推定される推定負荷トルクに切り替え、ステップＳ１０４において、位置指令生成部１０からの位置指令値と位置フィードバックの検出位置との差を、補正量生成部３０１で計算された補正量で補正する。そして、ステップＳ１０６で補正された、位置指令値と検出位置との差に基づいてモータ制御部２０によりサーボモータ５０を制御する。
位置指令加速度によりスイッチ３０３の切り替えの判断を行う場合には、ステップＳ１０２の位置指令速度を位置指令加速度に変更すればよい。

（第２の実施形態）
第１の実施形態のサーボモータ制御装置では、スイッチ３０３で固定値から推定負荷トルクに切り替えると、切り替え時に補正量が不連続に変わる。この補正量の不連続を防止するために位置指令補正部３１の出力にフィルタ１１０を設ける。
図９は本発明の第２の実施形態となるサーボモータ制御装置の構成を示すブロック図である。図９のサーボモータ制御装置の構成は、フィルタ１１０が設けられている点を除くと図３のサーボモータ制御装置の構成と同じであり、同一の構成部材については同一符号を付し説明を省略する。フィルタ１１０としてはローパスフィルタを用いることができる。

図１０に示す切り替えスイッチ１１１により、切り替え部となるスイッチ３０３で固定値から推定負荷トルクに切り替える時のみ補正量生成部３０１と加算器９０との間にフィルタ１１０を接続し、スイッチ３０３による切り替え後は切り替えスイッチ１１１により補正量生成部３０１と加算器９０との間を接続してフィルタ１１０を介さないようにしてもよい。切り替えスイッチ１１１の切り替えは図３等に示す位置指令速度判断部１１１による指示信号に基づいて行い、スイッチ３０３により固定値から推定負荷トルクに切り替える時に切り替えスイッチ１１１の切り替えを行う。
図１１はフィルタ１１０の切り替え動作を示すフローチャートである。ステップＳ２０１で位置指令速度判断部１１１が位置指令値の指令速度が所望の値を超え、且つ切削送りと判断した場合に（ステップＳ２０１のＹＥＳ）、ステップ２０２で、スイッチ３０３により固定値から推定負荷トルクに切り替え、その時フィルタをかけるように切り替えスイッチ１１１を切り替え、その後、補正量の不連続が解消されたときに、ステップ２０３でフィルタをかけないように切り替えスイッチ１１１を切り替える。位置指令加速度によりスイッチ３０３の切り替えの判断を行う場合には、ステップＳ２０１の位置指令速度を位置指令加速度に変更すればよい。

（第３の実施形態）
第２の実施形態のサーボモータ制御装置では、補正量の不連続を防止するために位置指令補正部３１の出力にフィルタ１１０を設けたが、フィルタの代わりにトルク指令作成部２０２の積分器２０１１の書き換えを行い、トルク指令値に不連続を生じないようにしてもよい。再開時に位置指令値を補正する補正量が入ると、速度指令値の変化が発生する。この速度指令値の変化によりトルク指令値が変化しないように、積分器の書き換えを行い、トルク指令値が不連続でなくなるようにする。
図１２は本発明の第３の実施形態となるサーボモータ制御装置の構成を示すブロック図である。図１２のサーボモータ制御装置の構成は、位置指令速度判断部１２０がトルク指令作成部２０２に積分器の書き換え指示を送っている点を除くと図５のサーボモータ制御装置の構成と同じであり、同一の構成部材については同一符号を付し説明を省略する。図１３はトルク指令作成部２０２の構成を示すブロック図である。図１３に示すように、積分器２０２１に書き換え指示が送られる。

書き換え指示は、図３等に示す位置指令速度判断部１２０からの指示信号に基づいてスイッチ３０３により固定値から推定負荷トルクに切り替える時に送られる。書き換え指示はスイッチ３０３に送られる切り替え信号と同じ信号でよいが、切り替え信号と別途生成してもよい。積分器２０２１の書き換えは書き換え指示に基づいて以下のように行われる。
トルク指令値TCMDは、Vcmdを速度指令値、Vfbを検出速度、ｋｐを比例ゲイン、ｋｉを積分ゲインとすると、

であらわされる。
補正量の更新停止から補正量の更新を再開ときに、トルク指令値TCMDはTCMD(1)からTCMD(2)になる。

本実施形態では、トルク指令値TCMD(2)のTCMD(1)+(Vcmd(2) - Vfb(2))を以下の数３に書き換える。

すると、書き換えられたトルク指令値TCMD’(2)はTCMD(1)と同じになり、トルク指令の不連続は生じない。

図１４は積分器２０２１の切り替え動作を示すフローチャートである。ステップＳ３０１で位置指令速度判断部１２０が位置指令値の指令速度が所望の値を超え、且つ切削送りと判断した場合に（ステップＳ３０１のＹＥＳ）、ステップ３０２で、スイッチ３０３により固定値から推定負荷トルクに切り替え、その時に積分器の書き換えを行う。その後、補正量の不連続が解消されたときに、ステップ３０３で積分器の書き換えを元に戻す。
位置指令加速度によりスイッチ３０３の切り替えの判断を行う場合には、ステップＳ３０２の位置指令速度を位置指令加速度に変更すればよい。
以上説明した各実施形態においては、位置指令速度判断部１２０は、位置指令作成部１０が作成した位置指令値を受けて、位置指令値の指令速度（位置指令速度）を生成し、早送りか切削送りかを指示する信号を受けている。そして、位置指令速度判断部１２０は、指令速度（絶対値）が所望の値以下（０を含む）のときに、又は早送りのときに、連結部において駆動体に作用する駆動力（駆動トルク）を、力推定部３０２により推定された駆動力から固定値に切り替えるように、切り替え部となるスイッチ３０３に切り替え信号を送る。一方、位置指令速度判断部１２０は、指令速度（絶対値）が所望の値を超え、且つ切削送りのときに、連結部において駆動体に作用する駆動力（駆動トルク）を、力推定部３０２により推定された駆動力に切り替えるように、スイッチ３０３に切り替え信号を送る。
しかし、各実施形態においては、位置指令速度判断部１２０は、早送りか切削送りかを指示する信号を受けず、位置指令作成部１０が作成した位置指令値を受けて、位置指令値の指令速度（位置指令速度）を生成し、指令速度（絶対値）が所望の値以下（０を含む）のときに、連結部において駆動体に作用する駆動力（駆動トルク）を、力推定部３０２により推定された駆動力から固定値に切り替え、指令速度（絶対値）が所望の値を超えているときに、連結部において駆動体に作用する駆動力（駆動トルク）を、力推定部３０２により推定された駆動力に切り替えるように構成してもよい。

以上説明したように、各実施形態では、位置指令速度又位置指令加速度が所望の値以下、又は早送り時は推定負荷トルクをあらかじめ決めた固定値とする。早送り動作中は基本的に、負荷トルクは一定であり、これに基づいて早送り時も負荷トルクを固定値にして補正する。推定負荷トルクを固定値にしているので、ボールねじの長さに依存した補正を行うことができる。負荷トルクの更新を行わなくなるが、負荷トルクに対する弾性変形へのボールねじの長さの影響は反映できる。
一方、位置指令速度が所望の値を超える且つ切削送り時は、動作中のトルク指令より推定した負荷トルクを用いて補正する。
こうして、各実施形態では、サーボモータの動作と関係ない補正を行わず、機械のロストモーションのみを補正することができる。

以上、本発明の実施形態について説明したが、サーボモータ制御装置はその機能の全部又は一部をハードウェア、ソフトウェア又はこれらの組み合わせにより実現することができる。ここで、ソフトウェアによって実現されるとは、コンピュータがプログラムを読み込んで実行することにより実現されることを意味する。ハードウェアで構成する場合、サーボモータ制御装置の補正量生成部３０１、力推定部３０２、位置指令生成部１０、モータ制御部２０の一部又は全部を、例えば、ＬＳＩ(Large Scale Integrated circuit)、ＡＳＩＣ(Application Specific Integrated Circuit)、ゲートアレイ、ＦＰＧＡ(Field Programmable Gate Array)等の集積回路（ＩＣ）で構成することができる。
ソフトウェアによって実現する場合、サーボモータ制御装置の一部又は全部を、ＣＰＵとプログラムを記憶したハードディスク、ＲＯＭ等の記憶部とを含むコンピュータで構成して、図３−図７のブロック図と図８のフローチャートに沿ったプログラム、図９、図１０のブロック図と図１１のフローチャートに沿ったプログラム、図１２及び図１３のブロック図と図１４のフローチャートのフローチャートに沿ったプログラムに従い、演算に必要な情報をＲＡＭ等の第２の記憶部に記憶し、処理を実行することでサーボモータ制御装置の一部又は全部の動作をプログラムで実行ことができる。プログラムは、プログラムが記録されたＣＤ−ＲＯＭ、ＤＶＤ、フラッシュメモリ等の外部記憶媒体からハードディスク等の記憶部に読み込むことができる。

１０ 位置指令生成部
２０ モータ制御部
３０、３１ 位置指令補正部
４０ エンコーダ
５０ サーボモータ
６０ 連結機構
７０ テーブル
３０１ 補正量生成部
３０２ 力推定部
３０３ スイッチ

Claims (13)

1. サーボモータと、
   前記サーボモータにより駆動される被駆動体と、
   前記サーボモータと前記被駆動体とを連結し、前記サーボモータの動力を前記被駆動体へ伝える連結機構と、
   前記被駆動体の位置指令値を生成する位置指令生成部と、
   前記位置指令値を用いて前記サーボモータを制御するモータ制御部と、
   前記連結機構との連結部において前記被駆動体に作用する駆動力を推定する力推定部と、
   前記力推定部で推定した第１の力推定値と、固定値の第２の力推定値とを切り替える切り替え部と、
   前記切り替え部により切り替えられた前記第１又は第２の力推定値に基づき、前記位置指令生成部により生成された前記位置指令値を補正するため補正量を生成する補正量生成部と、
   を具備するサーボモータ制御装置。
2. 前記補正量生成部は、前記切り替え部により切り替えられた前記第１又は第２の力推定値と第１の係数との積と、前記切り替え部により切り替えられた前記第１又は第２の力推定値と前記サーボモータから前記連結部までの距離と第２の係数との積と、の和を前記補正量とする請求項１に記載のサーボモータ制御装置。
3. 前記第２の力推定値は、前記位置指令生成部で作成された位置指令の方向に対応する符号のあらかじめ定めた一定値である請求項１又は２に記載のサーボモータ制御装置。
4. 前記切り替え部は、前記位置指令生成部が作成した前記位置指令値の指令速度又は指令加速度が所望の値以下のときに、前記第２の力推定値を選択する請求項１から３のいずれか１項に記載のサーボモータ制御装置。
5. 前記切り替え部は、早送り動作時に、前記第２の力推定値を選択する請求項１から３のいずれか１項に記載のサーボモータ制御装置。
6. 前記切り替え部により前記第２の力推定値から前記第１の力推定値に切り替えるときに、前記補正量生成部が生成した前記補正量にフィルタをかける請求項１から５のいずれか１項に記載のサーボモータ制御装置。
7. 前記モータ制御部は、速度指令作成部とトルク指令作成部とを有し、
   前記トルク指令作成部は少なくとも速度偏差を積算する積分器を有し、
   前記切り替え部により前記第２の力推定値から前記第１の力推定値に切り替えるときに、前記積分器の書き換えを行う請求項１から５のいずれか１項に記載のサーボモータ制御装置。
8. サーボモータと、
   前記サーボモータにより駆動される被駆動体と、
   前記サーボモータと前記被駆動体とを連結し、前記サーボモータの動力を前記被駆動体へ伝える連結機構と、を備えたサーボモータ制御装置のサーボモータ制御方法において、
   前記被駆動体の位置指令値を生成し、
   前記連結機構との連結部において前記被駆動体に作用する駆動力を推定し、
   前記位置指令値の指令速度若しくは指令加速度が所望の値以下の場合又は早送りの場合に、推定した第１の力推定値から、固定値の第２の力推定値に切り替え、
   切り替えられた前記第２の力推定値に基づき、生成された前記位置指令値を補正し、
   補正された前記位置指令値を用いて前記サーボモータを制御するサーボモータ制御方法。
9. 前記位置指令値を補正するための位置指令補正値は、前記第２の力推定値と第１の係数との積と、前記第２の力推定値と前記サーボモータから前記連結部までの距離と第２の係数との積と、の和である請求項８に記載のサーボモータ制御方法。
10. 前記第２の力推定値は、位置指令の方向に対応する符号のあらかじめ定めた一定値である請求項８又は９に記載のサーボモータ制御方法。
11. 前記第２の力推定値から前記第１の力推定値に切り替えるときに、前記位置指令値の補正値にフィルタをかける請求項８から１０のいずれか１項に記載のサーボモータ制御方法。
12. 補正された前記位置指令値を用いて速度指令を作成し、作成された速度指令を用いてトルク指令を作成し、該トルク指令を用いて前記サーボモータを制御し、
    前記トルク指令の作成は少なくとも速度偏差を積算する積分器を用いて行い、
    前記第２の力推定値から前記第１の力推定値に切り替えるときに、前記積分器の書き換えを行う請求項８から１１のいずれか１項に記載のサーボモータ制御方法。
13. サーボモータと、
    前記サーボモータにより駆動される被駆動体と、
    前記サーボモータと前記被駆動体とを連結し、前記サーボモータの動力を前記被駆動体へ伝える連結機構と、を備えたサーボモータ制御装置のサーボモータ制御をコンピュータに実行させるサーボモータ制御用プログラムであって、
    前記コンピュータに、
    前記被駆動体の位置指令値を生成する処理と、
    前記連結機構との連結部において前記被駆動体に作用する駆動力を推定する処理と、
    前記位置指令値の指令速度若しくは指令加速度が所望の値以下の場合又は早送りの場合に、推定した第１の力推定値から、固定値の第２の力推定値に切り替える処理と、
    切り替えられた前記第２の力推定値に基づき、生成された前記位置指令値を補正する処理と、
    補正された前記位置指令値を用いて前記サーボモータを制御する処理と、
    を実行させるサーボモータ制御用プログラム。

Images