JP6370866B2 - サーボモータ制御装置、サーボモータ制御方法、及びサーボモータ制御用プログラム - 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 (15)

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

Images