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

Abstract

【課題】精度の高い被駆動体の位置制御が可能なサーボモータ制御装置を提供すること。
【解決手段】サーボモータ５０と、サーボモータにより駆動される被駆動体７０と、サーボモータと被駆動体とを連結し、サーボモータの動力を被駆動体へ伝える連結機構（６０１、６０２）と、被駆動体の位置指令値を生成する位置指令生成部１０と、位置指令値を用いてサーボモータを制御するモータ制御部２０と、連結機構との連結部において被駆動体に作用する駆動力である力推定値を推定する力推定部３０２と、位置指令値に基づいて、力推定値の更新の反映と更新の停止とを判断し、更新を反映した力推定値と、位置指令値に基づいて更新を停止した時の力推定値とのいずれかを出力する力推定値出力部３０３と、力推定値出力部の出力を用いて、位置指令値を補正するため補正量を生成する補正量生成部３０１と、を具備する。
【選択図】図３

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 (10)

1. サーボモータと、
   前記サーボモータにより駆動される被駆動体と、
   前記サーボモータと前記被駆動体とを連結し、前記サーボモータの動力を前記被駆動体へ伝える連結機構と、
   前記被駆動体の位置指令値を生成する位置指令生成部と、
   前記位置指令値を用いて前記サーボモータを制御するモータ制御部と、
   前記連結機構との連結部において前記被駆動体に作用する駆動力である力推定値を推定する力推定部と、
   前記位置指令値に基づいて、前記力推定値の更新の反映と更新の停止とを判断し、更新を反映した力推定値と、前記位置指令値に基づいて更新を停止した時の力推定値とのいずれかを出力する力推定値出力部と、
   前記力推定値出力部の出力を用いて、前記位置指令値を補正するため補正量を生成する補正量生成部と、
   を具備するサーボモータ制御装置。
2. 前記力推定値出力部は、前記位置指令値の指令速度、又は前記位置指令値の指令加速度が所定の値以下となった場合に、前記力推定部が推定した力推定値の更新の停止を判断する請求項１に記載のサーボモータ制御装置。
3. 前記力推定値出力部は、前記位置指令値の指令速度、又は前記位置指令値の指令加速度が所定の値以下となった場合に、前記更新を停止した時の力推定値を基準として、所定の力推定値範囲を規定し、前記力推定部から出力される力推定値が前記力推定値範囲内にあれば、前記更新を停止した時の力指令値を出力する請求項１又は２に記載のサーボモータ制御装置。
4. 前記補正量生成部は、前記力推定値出力部から出力された前記力推定値と第１の係数との積と、前記力推定値出力部から出力された前記力推定値と前記サーボモータから前記連結部までの距離と第２の係数との積と、の和を前記補正量とする請求項１から３のいずれか１項に記載のサーボモータ制御装置。
5. 前記力推定値範囲は、前記更新を停止した時の力推定値を基準として、前記力推定値の大小方向に対してそれぞれ異なる力推定値範囲を設けた請求項３に記載のサーボモータ制御装置。
6. 前記位置指令生成部により生成された位置指令値の指令速度又は指令加速度が所定の値を超える場合に、前記力推定値出力部から出力された力推定値から前記力推定部で推定した力推定値に切り替える切り替え部を具備する請求項１から５のいずれか１項に記載のサーボモータ制御装置。
7. 前記力推定値出力部が、出力する力推定値を、更新を反映した力推定値と、前記位置指令値に基づいて更新を停止した時の力推定値との間で切り替えるとき、又は前記切り替え部が、出力する力推定値を、前記力推定値出力部から出力される力推定値と、前記力推定部から出力される力推定値との間で切り替えるときに、前記補正量生成部が生成した前記補正量にフィルタをかける請求項１から６のいずれか１項に記載のサーボモータ制御装置。
8. 前記モータ制御部は、速度指令作成部とトルク指令作成部とを有し、
   前記トルク指令作成部は少なくとも速度偏差を積算する積分器を有し、
   前記力推定値出力部が、出力する力推定値を、更新を反映した力推定値と、前記位置指令値に基づいて更新を停止した時の力推定値との間で切り替えるとき、又は前記切り替え部が、出力する力推定値を、前記力推定値出力部から出力される力推定値と、前記力推定部から出力される力推定値との間で切り替えるときに、前記積分器の書き替えを行う請求項１から７のいずれか１項に記載のサーボモータ制御装置。
9. サーボモータと、
   前記サーボモータにより駆動される被駆動体と、
   前記サーボモータと前記被駆動体とを連結し、前記サーボモータの動力を前記被駆動体へ伝える連結機構と、を備えたサーボモータ制御装置のサーボモータ制御方法において、
   前記被駆動体の位置指令値を生成し、
   前記連結機構との連結部において前記被駆動体に作用する駆動力である力推定値を推定し、
   前記位置指令値に基づいて、前記力推定値の更新の反映と更新の停止とを判断し、更新を反映した力推定値と、前記位置指令値に基づいて更新を停止した時の力推定値とのいずれかを選択し、 選択された力推定値に基づき、生成された前記位置指令値を補正し、
   補正された前記位置指令値を用いて前記サーボモータを制御するサーボモータ制御方法。
10. サーボモータと、
    前記サーボモータにより駆動される被駆動体と、
    前記サーボモータと前記被駆動体とを連結し、前記サーボモータの動力を前記被駆動体へ伝える連結機構と、を備えたサーボモータ制御装置のサーボモータ制御をコンピュータに実行させるサーボモータ制御用プログラムであって、
    前記コンピュータに、
    前記被駆動体の位置指令値を生成する処理と、
    前記連結機構との連結部において前記被駆動体に作用する駆動力である力推定値を推定する処理と、
    前記位置指令値に基づいて、前記力推定値の更新の反映と更新の停止とを判断し、更新を反映した力推定値と、前記位置指令値に基づいて更新を停止した時の力推定値とのいずれかを選択する処理と、
    選択された力推定値に基づき、生成された前記位置指令値を補正する処理と、
    補正された前記位置指令値を用いて前記サーボモータを制御する処理と、
    を実行させるサーボモータ制御用プログラム。
    
    

Images