(57)【特許請求の範囲】
【請求項１】
一つの被駆動体をマスター軸のモータ及びスレーブ軸のモータで駆動するための制御を行なうサーボ制御装置において、
前記マスター軸の速度偏差の積分値と前記スレーブ軸の速度偏差の積分値との差分を算出する差分算出手段と、
前記差分をローパスフィルタでフィルタ処理するフィルタ手段と、
前記フィルタ処理の処理結果を、前記スレーブ軸の速度偏差の積分値に加算する加算手段と、
を備え、
前記マスター軸では該マスター軸の速度偏差の積分値を用いて該マスター軸のモータを駆動するための電流指令を算出し、前記スレーブ軸では前記加算手段による加算後の積分値を用いて該スレーブ軸のモータを駆動するための前記電流指令を算出することを特徴とするサーボ制御装置。
【請求項２】
前記マスター軸のモータに対応するマスター軸制御手段であって、マスター軸及びスレーブ軸で共通の速度指令と、自制御手段に対応するマスター軸のモータの速度フィードバックとに基づいて、自制御手段に対応するマスター軸のモータの速度偏差を算出すると共に、該算出した速度偏差に比例ゲインを乗じた値と、該算出した速度偏差を積分した積分値に積分ゲインを乗じた値との和を、自制御手段に対応するマスター軸を駆動するための電流指令として算出するマスター軸制御手段と、
前記スレーブ軸のモータに対応するスレーブ軸制御手段であって、前記マスター軸及びスレーブ軸で共通の速度指令と、自制御手段に対応するスレーブ軸のモータの速度フィードバックとに基づいて、自制御手段に対応するスレーブ軸のモータの速度偏差を算出すると共に、該算出した速度偏差に比例ゲインを乗じた値と、前記加算手段による加算後の積分値に積分ゲインを乗じた値との和を、自制御手段に対応するスレーブ軸を駆動するための電流指令として算出するスレーブ軸制御手段と、
を更に備えることを特徴とする請求項１に記載のサーボ制御装置。
【請求項３】
前記スレーブ軸をＮ軸（Ｎは２以上の自然数）備えると共に、該Ｎ軸のスレーブ軸に対応して、前記差分算出手段、前記フィルタ手段、及び前記加算手段をそれぞれＮ個備え、
前記Ｎ個の前記差分算出手段のそれぞれが、前記マスター軸の前記積分値と、自差分算出手段に対応するスレーブ軸の前記積分値との差分を算出し、
前記Ｎ個のフィルタ手段のそれぞれが、自フィルタ手段に対応するスレーブ軸の前記差分をローパスフィルタでフィルタ処理し、
前記Ｎ個の加算手段のそれぞれが、自加算手段に対応するスレーブ軸の前記フィルタ処理の処理結果を、自加算手段に対応するスレーブ軸の前記積分値に加算する、
ことにより前記Ｎ軸のスレーブ軸それぞれにおいて、自スレーブ軸に対応する前記加算手段による加算後の積分値を用いて自スレーブ軸のモータを駆動するための電流指令の算出を行うことを特徴とする請求項１又は２に記載のサーボ制御装置。
【請求項４】
前記マスター軸及び前記スレーブ軸間の機械的結合剛性を推定する推定手段を更に備え、
該推定手段の推定結果に基づいて、前記ローパスフィルタの遮断周波数を調整することを特徴とする請求項１乃至３の何れか１項に記載のサーボ制御装置。
【請求項５】
前記ローパスフィルタの遮断周波数を、マスター軸とスレーブ軸間の機械的結合剛性が高い場合に高くなるように調整し、マスター軸とスレーブ軸間の機械的結合剛性が低い場合に低くなるように調整することを特徴とする請求項４に記載のサーボ制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、サーボモータを制御するためのサーボ制御装置に関するものであり、特に、被駆動体を複数のサーボモータで駆動する場合での制御を行うサーボ制御装置に関する。
【背景技術】
【０００２】
ロボット、工作機械、射出成形機、及びプレス機械といった各種機械の駆動機構において、移動対象とする被駆動体が大型で、その移動軸を駆動するサーボモータが１つでは、加減速ができない場合や、モータの駆動軸に接続された伝達機構の機械要素と被駆動体との間のバックラッシが大きいために被駆動体を安定した状態で移動させることができない場合に、タンデム制御を利用することが従来から行われている。
かかるタンデム制御では、移動指令を複数のサーボモータに対して与え、１つの被駆動体を複数のサーボモータで駆動する。これにより、大型の被駆動体を駆動させることや、被駆動体を安定した状態で移動させることが可能となる。
【０００３】
このようなタンデム制御では、複数のサーボモータの内の何れか１つが、伝達機構に連結されたマスター軸としての役割を果たす駆動軸を有するメインのサーボモータ（以下、適宜「マスター側サーボモータ」と呼ぶ。）として機能し、その他のサーボモータが、伝達機構に連結されたスレーブ軸としての役割を果たす駆動軸を有するサブのサーボモータ（以下、適宜「スレーブ側サーボモータ）と呼ぶ。）として機能する。
この場合に、マスター側及びスレーブ側のそれぞれのサーボモータにおいて加減速を繰り返していると、各サーボモータにおける速度フィードバックの取り込みのタイミングの差等が原因で、各サーボモータの速度制御部の積分要素の積分値が互いに乖離することがある。このように各サーボモータの積分値が互いに乖離してしまうと、これに起因してサーボモータの制御性が悪化したり、過大なトルク指令がなされてサーボモータがオーバヒートしたりするおそれがある。
【０００４】
この問題を解決するための技術が、特許文献１に開示されている。特許文献１に開示の技術では、スレーブ側の速度積分器の積分値を、マスター側の速度積分器の積分値と一致させるように制御をする。具体的には、マスター側サーボモータに対する速度指令値とマスター側サーボモータの速度との差分である速度偏差の積分値を用いて、マスター側サーボモータに対するトルク指令値のみならずスレーブ側サーボモータに対するトルク指令値をも生成する。このようにすることにより、上述した各サーボモータ間での積分値の乖離という問題を解消することができる。
しかしながら、このようにマスター側及びスレーブ側の速度積分器それぞれの積分値を常に一致させると、スレーブ側サーボモータを主に駆動するとき、すなわち、マスター側サーボモータの加速度が負の値であるときのサーボモータに対する位置指令値とサーボモータの位置との間の位置偏差が大きくなり、サーボモータの制御に対して悪影響が及ぼされ、制御が不安定となる。
【０００５】
この点を鑑みて、更に改良された技術が特許文献２に開示されている。特許文献２に開示の技術では、スレーブ側サーボモータを主に駆動するときに生じる上記問題を解消するために、加速方向に応じて、マスター側及びスレーブ側の速度積分器の何れかを選択し、選択した速度積分器の積分値を、他方の速度積分器の積分値に一致させる。すなわち、加速方向に応じて、積分値のコピー元をマスター側とするかスレーブ側とするかを切り替える。
これにより、スレーブ側サーボモータを主に駆動するときには、マスター側サーボモータの速度積分器の積分値を、スレーブ側サーボモータの速度積分器の積分値と一致させることができ、特許文献１に記載の構成から生じる上記問題を解消することができる。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特許第３５３７４１６号公報
【特許文献２】特許第５６４２８４８号公報
【特許文献３】特開２００９−８３０７４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
上述したように、特許文献２に開示の技術等を利用するにより、加速方向に応じて、マスター側及びスレーブ側それぞれの速度積分器の積分値を常に一致させることが可能となる。
しかしながら、加減速の速い動きを行っているにも関わらず、積分値を常に一致させてしまうと、マスター側及びスレーブ側の両軸と被駆動体との機械的結合剛性が低い場合には、被駆動体が弾性変形することにより、被駆動体に「たわみ」や「ねじれ」といった問題が生じ、被駆動体を適切に制御することができなくなるため問題となる。
【０００８】
そこで、本発明は、被駆動体に対する制御性を保ちつつ、速度積分器の積分値を調整することが可能な、サーボモータ制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明によれば、一つの被駆動体をマスター軸のモータ及びスレーブ軸のモータで駆動するための制御を行なうサーボ制御装置において、前記マスター軸の速度偏差の積分値と前記スレーブ軸の速度偏差の積分値との差分を算出する差分算出手段と、前記差分をローパスフィルタでフィルタ処理するフィルタ手段と、前記フィルタ処理の処理結果を、前記スレーブ軸の速度偏差の積分値に加算する加算手段と、を備え、前記マスター軸では該マスター軸の速度偏差の積分値を用いて該マスター軸のモータを駆動するための電流指令を算出し、前記スレーブ軸では前記加算手段による加算後の積分値を用いて該スレーブ軸のモータを駆動するための前記電流指令を算出することを特徴とするサーボ制御装置が提供される。
【００１０】
更に好適には、前記マスター軸のモータに対応するマスター軸制御手段であって、マスター軸及びスレーブ軸で共通の速度指令と、自制御手段に対応するマスター軸のモータの速度フィードバックとに基づいて、自制御手段に対応するマスター軸のモータの速度偏差を算出すると共に、該算出した速度偏差に比例ゲインを乗じた値と、該算出した速度偏差を積分した積分値に積分ゲインを乗じた値との和を、自制御手段に対応するマスター軸を駆動するための電流指令として算出するマスター軸制御手段と、前記スレーブ軸のモータに対応するスレーブ軸制御手段であって、前記マスター軸及びスレーブ軸で共通の速度指令と、自制御手段に対応するスレーブ軸のモータの速度フィードバックとに基づいて、自制御手段に対応するスレーブ軸のモータの速度偏差を算出すると共に、該算出した速度偏差に比例ゲインを乗じた値と、前記加算手段による加算後の積分値に積分ゲインを乗じた値との和を、自制御手段に対応するスレーブ軸を駆動するための電流指令として算出するスレーブ軸制御手段と、を更に備えるサーボ制御装置とするとよい。
【００１１】
更に好適には、前記スレーブ軸をＮ軸（Ｎは２以上の自然数）備えると共に、該Ｎ軸のスレーブ軸に対応して、前記差分算出手段、前記フィルタ手段、及び前記加算手段をそれぞれＮ個備え、前記Ｎ個の前記差分算出手段のそれぞれが、前記マスター軸の前記積分値と、自差分算出手段に対応するスレーブ軸の前記積分値との差分を算出し、前記Ｎ個のフィルタ手段のそれぞれが、自フィルタ手段に対応するスレーブ軸の前記差分をローパスフィルタでフィルタ処理し、前記Ｎ個の加算手段のそれぞれが、自加算手段に対応するスレーブ軸の前記フィルタ処理の処理結果を、自加算手段に対応するスレーブ軸の前記積分値に加算する、ことにより前記Ｎ軸のスレーブ軸それぞれにおいて、自スレーブ軸に対応する前記加算手段による加算後の積分値を用いて自スレーブ軸のモータを駆動するための電流指令の算出を行うサーボ制御装置とするとよい。
【００１２】
更に好適には、前記マスター軸及び前記スレーブ軸間の機械的結合剛性を推定する推定手段を更に備え、該推定手段の推定結果に基づいて、前記ローパスフィルタの遮断周波数を調整するサーボ制御装置とするとよい。
【００１３】
更に好適には、前記ローパスフィルタの遮断周波数を、マスター軸とスレーブ軸間の機械的結合剛性が高い場合に高くなるように調整し、マスター軸とスレーブ軸間の機械的結合剛性が低い場合に低くなるように調整するサーボ制御装置とするとよい。
【発明の効果】
【００１４】
本発明によれば、被駆動体に対する制御性を保ちつつ、速度積分器の積分値を調整することが可能となる。
【図面の簡単な説明】
【００１５】
【図１】本発明の第１の実施形態全体の構成を示すブロック図である。
【図２】本発明の第１の実施形態における速度制御部の構成を示すブロック図である。
【図３】本発明の各実施形態におけるマスター軸の動作を示すフローチャートである。
【図４】本発明の各実施形態におけるスレーブ軸の動作を示すフローチャートである。
【図５】本発明の第２の実施形態全体の構成を示すブロック図である。
【図６】本発明の第２の実施形態における速度制御部の構成を示すブロック図である。
【図７】本発明の第３の実施形態全体の構成を示すブロック図である。
【図８】本発明の第３の実施形態における速度制御部の構成を示すブロック図である。
【発明を実施するための形態】
【００１６】
以下、本発明の実施形態について図面を参照して説明をする。以下では、本発明の実施形態の一例として３つの実施形態について説明を行う。
３つの実施形態の概略であるが、第１の実施形態は、１つのマスター軸と、１つのスレーブ軸とにより１つの被駆動体を駆動させる実施形態である。
また、第２の実施形態は、スレーブ軸の数を増加させ、１つのマスター軸と、複数のスレーブ軸とにより１つの被駆動体を駆動させる実施形態である。
更に、第３の実施形態は、マスター側及びスレーブ側の両軸と被駆動体との機械的結合剛性を推定し、推定結果に基づいてローパスフィルタを調整する機能を追加した実施形態である。
【００１７】
＜第１の実施形態＞
まず、第１の実施形態について説明を行う。図１に示すように本実施形態は、上位制御装置１、位置制御部２、速度制御部１００、第１電流制御部３−１、第１電流増幅器４−１、第１速度検出器５−１、第１サーボモータ６−１、第１伝達機構７−１、第２電流制御部３−２、第２電流増幅器４−２、第２速度検出器５−２、第２サーボモータ６−２、第２伝達機構７−２、被駆動体８及び位置検出器９を備える。
【００１８】
本実施形態では、第１サーボモータ６−１及び第２サーボモータ６−２の２つのサーボモータによって１つの被駆動体８を駆動する駆動機構のタンデム制御を行う。また、これに伴い、速度制御部１００において、本実施形態特有の処理を行うことにより、被駆動体に対する制御性を保ちつつ、スレーブ軸の速度積分器の積分値を修正する。なお、以下の説明では、第１サーボモータ６−１をマスター側サーボモータとして扱い、第２サーボモータ６−２をスレーブ側サーボモータとして扱う。
【００１９】
上位制御装置１は、動作プログラム等に基づいて所定周期毎に各軸サーボモータに対して位置指令、あるいは位置指令の差分である移動指令ＭＣＭＤを位置制御部２に対して出力する。
本実施形態では、第１サーボモータ６−１及び第２サーボモータ６−２に対してタンデム制御を行い、これら２つのサーボモータに対する位置指令、あるいは移動指令ＭＣＭＤとしては同一の値の指令がなされる。
位置制御部２よりも後段の各部は、この位置指令、あるいは移動指令ＭＣＭＤを読みとり、所定周期毎に、位置制御、速度制御、及び電流制御を行い、第１電流増幅器４−１や第２電流増幅器４−２を介して、第１サーボモータ６−１及び第２サーボモータ６−２を駆動する。なお、上位制御装置１は、例えば、数値制御装置（ＣＮＣ：Computer Numerical Control)により実現することができる。また、以下の説明では、上位制御装置１から、位置指令が出力されるものとする。
【００２０】
上位制御装置１と位置制御部２の間には減算器が配置され、この減算器により、上位制御装置１が出力した位置指令から、後述の位置検出器９が検出した位置フィードバック量（図中では、フィードバック量を単に「ＦＢ」と表記する。）が減算される。そして、減算器は、減算後の値を位置制御部２に対して出力する。ここで、かかる減算後の値は、位置偏差量となる。そして、位置制御部２は、この位置偏差量を処理して速度指令を生成し、生成した速度指令を速度制御部１００に対して出力する。この際、出力された速度指令は分岐され、速度制御部１００内のマスター軸側の速度制御部分と、スレーブ軸側の速度制御部分の、それぞれに入力される。
このように、本実施形態では、マスター軸側及びスレーブ軸側の双方について共通の速度指令を出力することとしているので、位置検出器９を１つのみ配置し、この１つの位置検出器９が検出した１つの位置フィードバック量により減算を行なう。ただし、これは、位置検出器９の数を１つだけとすることに限定する趣旨ではなく、任意の用途のために、位置検出器９を複数配置するようにしてもよい。
【００２１】
速度制御部１００は、位置制御部２から速度指令を受け取り、速度指令から、後述の第１速度検出器５−１で検出された第１速度フィードバック量を差し引いた速度偏差量に基づいて、比例演算や積分演算を行なうといった速度ループ処理によりマスター軸側の電流指令である第１電流指令を出力する。同様に、速度制御部１００は、位置制御部２から速度指令を受け取り、速度指令から、後述の第２速度検出器５−２で検出された第２速度フィードバック量を差し引いた速度偏差量に基づいて、比例演算や積分演算を行なうといった速度ループ処理によりスレーブ軸側の電流指令である第２電流指令を出力する。なお、電流指令は、トルク指令と呼ばれることもある。
また、速度制御部１００は、このような速度ループ処理に加えて、本実施形態特有の処理を行なうことにより、スレーブ軸側の積算値を調整する。この点は、本実施形態の要旨の１つであるので、図１を参照した本説明を終了した後に、図２の機能ブロック図と、図３及び図４のフローチャートとを参照して詳細に説明する。
【００２２】
速度制御部１００と、第１電流制御部３−１及び第２電流制御部３−２それぞれの間には減算器が配置され、この減算器により、第１電流指令や第２電流指令から、各サーボモータのモータ電流を検出するセンサ（図示を省略する。）からの電流フィードバック量が減算される。
具体的には、第１電流指令から、第１サーボモータ６−１のモータ電流である第１電流フィードバック量が減算される。そして、減算器は、減算後の値を第１電流御部３−１に対して出力する。
また、同様にして第２電流指令から、第２サーボモータ６−２のモータ電流である第２電流フィードバック量が減算される。そして、減算器は、減算後の値を第２電流制御部３−２に対して出力する。
ここで、これら減算後の値は、それぞれが電流偏差量となる。そして、第１電流制御部３−１は、受け取った電流偏差量を処理して第１電圧指令を生成し、生成した第１電圧指令を第１電流増幅器４−１に対して出力する。
また、同様にして、そして、第２電流制御部３−２は、受け取った電流偏差量を処理して第２電圧指令を生成し、生成した第２電圧指令を第２電流増幅器４−２に対して出力する。
【００２３】
第１電流増幅器４−１は、受け取った第１電圧指令に基づいて、第１サーボモータ６−１を駆動する駆動電流を形成し、この駆動電流により第１サーボモータ６−１を駆動させる。また、同様にして、第２電流増幅器４−２は、受け取った第２電圧指令に基づいて、第２サーボモータ６−２を駆動する駆動電流を形成し、この駆動電流により第２サーボモータ６−２を駆動させる。
【００２４】
ここで、第１サーボモータ６−１に接続された第１伝達機構７−１と、第２サーボモータ６−２に接続された第２伝達機構７−２とは被駆動体８と接続されており、被駆動体８は、これら２つのモータの出力トルクの合力によって駆動する。例えば、第１伝達機構７−１と、第２伝達機構７−２をポールねじにより実現し、このポールねじと被駆動体８に取り付けられているボールナットと螺合することにより、被駆動体８との接続を実現することができる。
また、第１サーボモータ６−１及び第２サーボモータ６−２のそれぞれは、例えば、ロータとステータのうちのいずれか一方に永久磁石が設けられた回転型サーボモータ、ステータとスライダのうちのいずれか一方に永久磁石が設けられたリニアサーボモータ、ステータとバイブレータのうちのいずれか一方に永久磁石が設けられた振動型サーボモータ、永久磁石を使用しない誘導モータ等によって実現することができる。
また、被駆動体８は、例えば、工作機械のテーブル、産業用ロボットのアーム等によって実現することができる。
【００２５】
他方、第１サーボモータ６−１には、第１速度検出器５−１が取り付けられており、第１速度検出器５−１が検出した第１速度フィードバック量が、速度制御部１００のマスター軸側の速度制御部分に対して出力される。更に、同様に、第２サーボモータ６−２には、第２速度検出器５−２が取り付けられており、第２速度検出器５−２が検出した第２速度フィードバック量が、速度制御部２００のスレーブ軸側の速度制御部分に対して出力される。
【００２６】
位置検出器９は、第１サーボモータ６−１側の被駆動体８の位置を検出できるように配置される。かかる位置検出器９は被駆動体８に取り付けられてもよい。ただし、剛性等の条件によっては被駆動体８には取り付けず、第１サーボモータ６−１の出力軸に設置されるようにしてもよい。
すなわち、位置検出器９は、被駆動体の移動を直接検出するように被駆動体に取り付けられる場合もあり、モータの出力軸等に取り付けられ、モータの回転位置を検出することによって被駆動体の位置を検出する場合もある。更に、この位置検出器９を、例えばリニアスケールで構成するようにしてもよく、ロータリエンコーダで構成するようにしてもよい。
【００２７】
以上、図１を参照して説明した構成により、上位制御装置１からの位置指令、あるいは移動指令ＭＣＭＤを読みとり、所定周期毎に、位置制御、速度制御、及び電流制御を行い、第１電流増幅器４−１や第２電流増幅器４−２を介して、第１サーボモータ６−１及び第２サーボモータ６−２を駆動することが可能となる。
【００２８】
次に、速度制御部１００の構成及び速度制御部１００での処理について、図２の機能ブロック図と、図３及び図４のフローチャートとを参照して説明を行なう。
ここで、図２に示すように、速度制御部１００は、マスター軸側の速度制御部分として、減算器１１、積分器１２、比例ゲインの増幅器１３、積分ゲインの増幅器１４及び加算器１５を備える。また、速度制御部１００は、スレーブ軸側の速度制御部分として、減算器２１、積分器２２、比例ゲインの増幅器２３、積分ゲインの増幅器２４及び加算器２５を備える。更に、速度制御部１００は、スレーブ軸側の積分値を調整するための構成として、減算器３１、ローパスフィルタ３２及び加算器３３を備える。速度制御部１００は、ＰＩ制御を行なうことにより、電流指令を生成する。
【００２９】
次に、これら各構成要素の処理の内、マスター軸側の速度制御部分の動作について図３を参照して説明をする。
【００３０】
まず、減算器１１において速度指令値から第１速度フィードバック量を減算することによりマスター軸側の速度偏差を算出する。そして、減算器１１は、算出したマスター軸側の速度偏差を積分器１２及び比例ゲインの増幅器１３に対して出力する（ステップＳ１１）。
【００３１】
次に、比例ゲインの増幅器１３は、入力されたマスター軸側の速度偏差と、この速度偏差を増幅するための比例ゲインとの積を算出し、算出結果を加算器１５に対して出力する（ステップＳ１２）。比例ゲインの値は、予め比例ゲインの増幅器１３に設定されているものとする。
【００３２】
他方で、積分器１２は、入力されたマスター軸側の速度偏差に基づいて積分を行なうことにより、マスター軸側の速度偏差の積分値を算出する。算出したマスター軸側の速度偏差の積分値は、積分ゲインの増幅器１４及び減算器３１に対して出力する（ステップＳ１３）。
【００３３】
次に、積分ゲインの増幅器１４は、入力されたマスター軸側の速度偏差の積分値と、この積分値を増幅するための積分ゲインとの積を算出し、算出結果を加算器１５に対して出力する（ステップＳ１４）。積分ゲインの値は、予め積分ゲインの増幅器１４に設定されているものとする。
なお、図示の都合上、ステップＳ１２を行った後に、ステップＳ１３及びステップＳ１４が行なわれるように記載されているが、実際には、ステップＳ１２と、ステップＳ１３及びステップＳ１４は、同じタイミングで並行して行なわれる。
【００３４】
加算器１５は、入力された、マスター軸側の速度偏差と比例ゲインの積と、マスター軸側の速度偏差の積分値と積分ゲインの積とを加算することにより、第１電流指令を生成する。そして、加算器１５は、生成した第１電流指令を第１電流制御部３−１に対して出力する（ステップＳ１５）。出力された第１電流指令は、上述したように第１電流制御部３−１との間にある減算器により、第１電流フィードバック量が減算された後に、第１電流制御部３−１に入力される。
【００３５】
次に、スレーブ軸側の速度制御部分の動作及び減算器３１、ローパスフィルタ３２及び加算器３３により行なわれる積分値の調整動作について図４を参照して説明をする。
【００３６】
まず、減算器２１において速度指令値から第２速度フィードバック量を減算することによりマスター軸側の速度偏差を算出する。そして、積分器２２は、算出したマスター軸側の速度偏差を積分器２２及び比例ゲインの増幅器２３に対して出力する（ステップＳ２１）。なお、位置制御部２の説明の際にも述べたが、減算器１１に入力される速度指令と、減算器２１に入力される速度指令は同じものである。
【００３７】
次に、比例ゲインの増幅器２３は、入力されたスレーブ軸側の速度偏差と、この速度偏差を増幅するための比例ゲインとの積を算出し、算出結果を加算器２５に対して出力する（ステップＳ２２）。比例ゲインの値は、予め比例ゲインの増幅器２３に設定されているものとする。
【００３８】
他方で、積分器２２は、入力されたスレーブ軸側の速度偏差に基づいて積分を行なうことにより、スレーブ軸側の速度偏差の積分値を算出する。算出したスレーブ軸側の速度偏差の積分値は、積分ゲインの増幅器２４及び減算器３１に対して出力する（ステップＳ２３）。
減算器３１は、積分器１２から入力されたマスター軸の積分値から、積分器２２から入力されたスレーブ軸の積分値を減算することにより、積分値の差分を算出して、算出した積分値の差分をローパスフィルタ３２に対して出力する（ステップＳ２４）。
【００３９】
ローパスフィルタ３２は、入力された積分値の差分が、自身の時定数により定まる遮断周波数より低い周波数であれば通過させ、遮断周波数より高い周波数であれば減衰させるというフィルタ処理を行なう。そして、フィルタ処理の処理結果を加算器３３に出力する（ステップＳ２５）。
【００４０】
ここで、ローパスフィルタ３２の時定数により定まる遮断周波数は、スレーブ軸とマスター軸間の、第１伝達機構７−１、第２伝達機構７−２及び被駆動体８を介した機械的結合剛性（以下、適宜「機械的結合剛性」と呼ぶ。）に基づいて定めるようにするとよい。例えば、かかる機械的結合剛性が高い場合は遮断周波数も高くなるようにするとよく、これに比較してかかる機械的結合剛性が低い場合は遮断周波数も低くなるようにするとよい。これは、所望の遮断周波数となる時定数のローパスフィルタ３２を利用することにより実現できる。また、このようにするのではなく、所望の遮断周波数となる時定数になるように、時定数を調整する調整器を備えたローパスフィルタ３２を利用するようにしてもよい。
【００４１】
次に、加算器３３は、ローパスフィルタ３２から入力されたフィルタ処理の処理結果と、積分器２２から入力されたスレーブ軸側の速度偏差の積分値とを加算することにより、調整後の積分値を算出する。算出した調整後の積分値は積分ゲインの増幅器２４に出力する（ステップＳ２６）。
【００４２】
そして、積分ゲインの増幅器２４は、入力された調整後の積分値と、この積分値を増幅するための積分ゲインとの積を算出し、算出結果を加算器２５に対して出力する（ステップＳ２７）。積分ゲインの値は、予め積分ゲインの増幅器２４に設定されているものとする。
なお、図示の都合上、ステップＳ２２を行った後に、ステップＳ２３からステップＳ２６までが行なわれるように記載されているが、実際には、ステップＳ２２と、ステップＳ２３からステップＳ２６までは、同じタイミングで並行して行なわれる。
【００４３】
加算器２５は、入力された、スレーブ軸側の速度偏差と比例ゲインの積と、調整後の積分値と積分ゲインの積とを加算することにより、第２電流指令を生成する。そして、加算器２５は、生成した第２電流指令を第２電流制御部３−２に対して出力する（ステップＳ２８）。出力された第２電流指令は、上述したように第２電流制御部３−２との間にある減算器により、第２電流フィードバック量が減算された後に、第２電流制御部３−２に入力される。
なお、図３及び図４に示した各ステップは、所定周期毎に繰り返し実行される。
【００４４】
このような処理を行なうことにより、本実施形態では、スレーブ軸側の積分値をマスター軸側の積分値に一致させることが可能となる。
もっとも、本実施形態では、特許文献１や特許文献２に開示の技術のように、常にスレーブ軸側の積分値をマスター軸側の積分値に一致させる訳ではない。この点について以下に説明をする。
ローパスフィルタ３２の説明で述べたように、ローパスフィルタ３２に入力された積分値の差分が、遮断周波数より低い周波数であれば、この積分値の差分はローパスフィルタ３２を通過する。よって、この積算値の差分は加算器３３において、積分器２２から入力されるスレーブ軸側の積分値に加算される。すなわち、スレーブ軸側の積分値が調整され、マスター軸側の積分値と一致する積分値となる。
一方で、ローパスフィルタ３２に入力された積分値の差分が、遮断周波数より高い周波数であれば、この積分値の差分はローパスフィルタ３２により減衰する。よって、この積算値の差分は加算器３３において、積分器２２から入力されるスレーブ軸側の積分値には加算されない。すなわち、スレーブ軸側の積分値は調整されず、マスター軸側の積分値と異なる積分値となる。
【００４５】
ここで、第１サーボモータ６−１や第２サーボモータ６−２を駆動させることにより加減速等の速い動きで被駆動体８が駆動している場合のような、マスター軸側の積分値とスレーブ軸側の積分値の差分が大きくなるような場合に、スレーブ軸側の積分値を調整してマスター軸側の積分値と一致する積分値としてしまうと、特にスレーブ軸とマスター軸間の機械的結合剛性が低い場合に、被駆動体８に「ねじれ」等が発生して制御性が悪化する可能性がある。そのため、本実施形態では、ローパスフィルタ３２を利用することにより、マスター軸側の積分値とスレーブ軸側の積分値の差分が大きいような場合には、積分器２２が出力するスレーブ軸側の積分値を調整せず、積分器２２が出力するスレーブ軸側の積分値に基づいて第２電流指令を生成する。これにより、機械的結合剛性が低い場合に、被駆動体８に「ねじれ」等が発生して制御性が悪化することが防止できる。
一方で、第１サーボモータ６−１や第２サーボモータ６−２をゆっくりと駆動させている場合や、第１サーボモータ６−１や第２サーボモータ６−２を停止させた場合のような、マスター軸側の積分値とスレーブ軸側の積分値の差分が小さくなるような場合に、スレーブ軸側の積分値を調整してマスター軸側の積分値と一致する積分値とする。これにより、スレーブ軸側の積分値を調整してマスター軸側の積分値の乖離を抑制し、積分値が乖離した場合に生じてしまうトルク指令の増大という問題を防止することができる。
また、上述したようにスレーブ軸とマスター軸間の機械的結合剛性に応じてローパスフィルタ３２の遮断周波数を決定することにより、「ねじれ」が発生しない範囲において、スレーブ軸側の積分値を調整してマスター軸側の積分値と一致させることができる。
従って、本実施形態では、スレーブ軸側の積分値を調整してマスター軸側の積分値と一致することができると共に、常にスレーブ軸側の積分値を調整してマスター軸側の積分値と一致する積分値としてしまうと「ねじれ」等が発生してしまうという弊害を防止することが可能となる、という効果を奏する。
【００４６】
＜第２の実施形態＞
次に、上述した第１の実施形態を変形した第２の実施形態について図面を参照して説明する。ここで、第１の実施形態では、１つのマスター軸と、１つのスレーブ軸とにより１つの被駆動体を駆動させていた。一方で、第２の実施形態では、スレーブ軸の数をＮ軸（Ｎは２以上の自然数である。）に増加させ、１つのマスター軸と、Ｎ軸のスレーブ軸により１つの被駆動体を駆動させる点で相違する。
なお、その他の基本的な構成や処理において第１の実施形態と、第２の実施形態は共通する。そこで、以下の説明では、第１の実施形態との相違点についてもっぱら説明を行い、相違点についての説明は、第１の実施形態と重複するため省略する。
【００４７】
図５を参照すると、本実施形態は、マスター軸に対応する第１電流制御部３−１、第１電流増幅器４−１、第１速度検出器５−１、第１サーボモータ６−１、及び第１伝達機構７−１に加えて、Ｎ軸のスレーブ軸に対応する数だけこれらの構成要素を備える。図中には、第Ｍ電流制御部３−Ｍ（Ｍ＝Ｎ＋１である。）、第Ｍ電流増幅器４−Ｍ、第Ｍ速度検出器５−Ｍ、第Ｍサーボモータ６−Ｍ、及び第Ｍ伝達機構７−Ｍを図示する。
また、これに対応して速度制御部１００が、速度制御部１０１に置き換わっている。また、速度制御部１０１には、第１速度フィードバック量に加えて、第２速度フィードバック量から第Ｍ速度フィードバック量までが入力される。
【００４８】
次に、図６を参照して速度制御部１０１の構成について説明を行なう。図６に示すように、本実施形態の速度制御部１０１は、マスター軸側の速度制御部分に加えて、Ｎ軸分のスレーブ軸に対応したスレーブ軸側の速度制御部分を備える。図中には、スレーブ軸側の速度制御部分として、減算器Ｍ１、積分器Ｍ２、比例ゲインの増幅器Ｍ３、積分ゲインの増幅器Ｍ４及び加算器Ｍ５を図示する。また、これに対応して、速度制御部１０１は、スレーブ軸側の積分値を調整するための構成として、減算器３１、ローパスフィルタ３２及び加算器３３をＮ個備える。つまり、Ｎ軸のスレーブ軸のそれぞれについて、減算器３１、ローパスフィルタ３２及び加算器３３を備える。
【００４９】
これにより、各スレーブ軸側の速度制御部分では、自スレーブ軸の速度偏差に比例ゲインを乗じることができる。また、自スレーブ軸の速度偏差とマスター軸の速度偏差との差分を、自スレーブ軸の速度偏差に加算した後に積分ゲインを乗じることができる。そして、これら乗じた後の値を合算して電流指令を算出することができる。
すなわち、Ｎ軸のスレーブ軸のそれぞれが、他のスレーブ軸ではなく、自スレーブ軸の速度偏差に基づいた電流指令を算出することができる。これにより、本実施形態では、各スレーブ軸それぞれで、各スレーブ軸それぞれに適した制御を行なうことが可能となる、という効果を奏する。
【００５０】
＜第３の実施形態＞
次に、第３の実施形態について図面を参照して説明する。なお、以下では、第１の実施形態を変形して第３の実施形態とした場合について説明するが、上述した第２の実施形態のスレーブ軸をＮ軸とした構成を変形して第３の実施形態とするようにしてもよい。
ここで、本実施形態は、スレーブ軸とマスター軸間の、第１伝達機構７−１、第２伝達機構７−２及び被駆動体８を介した機械的結合剛性を推定し、この推定結果に基づいてローパスフィルタの時定数により定まる遮断周波数を調整する機能を追加した実施形態である。
なお、その他の基本的な構成や処理において本実施形態は、第１の実施形態や第２の実施形態と共通する。そこで、以下の説明では、第１の実施形態や第２の実施形態との相違点についてもっぱら説明を行い、相違点についての説明は、第１の実施形態や第２の実施形態の説明と重複するため省略する。
【００５１】
図７を参照すると、本実施形態では、スレーブ軸とマスター軸間の機械的結合剛性を推定するために、位置検出器９がマスター軸側のみならずスレーブ軸側にも設けられる。具体的には、図中に示すように、マスター軸側の位置検出器９として第１位置検出器９−１が設けられ、マスター軸側の位置検出器９として第２位置検出器９−２が設けられる。なお、第１位置検出器９−１及び第２位置検出器９−２の機能は、位置検出器９と同様である。また、速度制御部１００は、スレーブ軸とマスター軸間の機械的結合剛性の推定を行なう機能を追加した速度制御部１０２に置き換わっている。更に、第１位置検出器９−１及び第２位置検出器９−２がそれぞれ検出する２つの位置フィードバック量に対応するように、マスター軸側の位置制御部２として第１位置制御部２−１が設けられ、スレーブ軸側の位置制御部２として第２位置制御部２−２が設けられる。なお、第１位置制御部２−１及び第２位置制御部２−２の機能は、位置制御部２と同様である。
【００５２】
そして、スレーブ軸とマスター軸間の機械的結合剛性を推定するために、第１位置検出器９−１によって検出された位置フィードバック量は、第１位置フィードバック量として、速度制御部１０２内のマスター軸側の速度制御部分に入力される。また、第２位置検出器９−２によって検出された位置フィードバック量は、第２位置フィードバック量として、速度制御部１０２内のマスター軸側の速度制御部分に入力される。
【００５３】
また、上位制御装置１からは、第１位置制御部２−１及び第２位置制御部２−２に対して共通の位置指令が与えられるが、上位制御装置１と第１位置制御部２−１及び第２位置制御部２−２の間それぞれには減算器が配置され、この減算器により、上位制御装置１が出力した位置指令から、それぞれの位置フィードバック量が減算される。そして、各減算器は、減算後の値を第１位置制御部２−１と第２位置制御部２−２に対して出力する。
具体的には、マスター軸側の減算器は、位置指令から第１位置フィードバック量を減算した値を第１位置制御部２−１に対して出力する。また、スレーブ軸側の減算器は、位置指令から第２位置フィードバック量を減算した値を第２位置制御部２−２に対して出力する。ここで、かかる減算後の値は、それぞれ、マスター軸側の位置偏差量と、スレーブ軸側の位置偏差量となる。
そして、第１位置制御部２−１及び第２位置制御部２−２は、それぞれ自身が受け取った位置偏差量を処理して、それぞれの速度指令を生成し、生成した速度指令を速度制御部１００に対して出力する。具体的には、第１位置制御部２−１が出力した速度指令は第１速度指令として、速度制御部１０２内のマスター軸側の速度制御部分に入力される。また、第２位置制御部２−２が出力した速度指令は第２速度指令として、速度制御部１０２内のスレーブ軸側の速度制御部分に入力される。
【００５４】
なお、図７に示す構成にするのではなく、本実施形態においても、マスター軸側及びスレーブ軸側の速度指令を共通とするようにしてもよい。つまり、第１位置制御部２−１と第２位置制御部２−２を設けるのではなく、第１の実施形態や第２の実施形態と同様に、位置制御部２を１つ設ける。そして、位置制御部２に、上位制御装置１が出力した位置指令から第１位置フィードバック量又は第２フィードバック量の何れか一方を減算した値のみが入力されるようにして、マスター軸側及びスレーブ軸側の速度指令を共通とするようにしてもよい。
【００５５】
次に、図８を参照して速度制御部１０２の構成について説明を行なう。図８に示すように、速度制御部１０２は、速度制御部１００の構成に加えて、剛性推定調整部４０と、電流指令切替器４１を更に備える。
剛性推定調整部４０は、スレーブ軸とマスター軸間の機械的結合剛性を推定し、推定した機械的結合剛性に基づいてローパスフィルタ３２の時定数により定まる遮断周波数を調整する。
電流指令切替器４１は、速度制御部１０２が第１電流制御部３−１に出力する第１電流指令を、加算器１５が出力する位置制御用第１電流指令と、剛性推定調整部４０が出力する剛性推定用第１電流指令の何れにするかを切り替えるためのスイッチである。また、電流指令切替器４１は、同様に、速度制御部１０２が第２電流制御部３−２に出力する第２電流指令を、加算器２５が出力する位置制御用第２電流指令と、剛性推定調整部４０が出力する剛性推定用第４電流指令の何れにするかを切り替える。
【００５６】
なお、位置制御用第１電流指令は、実施形態１や実施形態２における第１電流指令と同じものであるが、剛性推定調整部４０が出力する剛性推定用第１電流指令と区別するために、便宜上「位置制御用」の文言を付す。位置制御用第２電流指令は、実施形態１や実施形態２における第２電流指令と同じものであるが、同様の理由で、便宜上「位置制御用」の文言を付す。
【００５７】
次に、剛性推定調整部４０によるスレーブ軸とマスター軸間の機械的結合剛性の推定方法について説明する。剛性推定調整部４０は、スレーブ軸とマスター軸間の機械的結合剛性として、例えば、ねじれ剛性を推定する。この点、ねじれ剛性の推定方法は、例えば、特許文献３に記載されている方法で行うことができる。
【００５８】
具体的には、剛性推定調整部４０は、位置制御を遮断して剛性推定を行なうために、電流指令切替器４１を切り替えて、位置制御用第１電流指令や位置制御用第２電流指令ではなく、剛性推定用第１電流指令と剛性推定用第２電流指令が出力されるようにする。ここで、剛性推定用第１電流指令と剛性推定用第２電流指令は、被駆動体８にねじれが生じるようにするための電流指令であり、例えば、特定周波数の正弦波状の測定トルクを、電流指令としたものである。
そして、剛性推定用第１電流指令と剛性推定用第２電流指令６−１により、第１サーボモータ及び第２サーボモータ６−２が駆動すると、これに応じて被駆動体８が駆動すると共に、被駆動体８にねじれが生じる。
この場合に、第１位置検出器９−１及び第２位置検出器９−２により検出された、第１位置フィードバック量及び第２位置フィードバック量が剛性推定調整部４０に入力される。
【００５９】
剛性推定調整部４０は、第１位置フィードバック量と第２位置フィードバック量との差分値に基づいて算出した被駆動体８のねじれ角と、剛性推定用第１電流指令及び剛性推定用第２電流指令それぞれの値とに基づいて、被駆動体８のねじれ剛性値を推定する。そして、推定したねじれ剛性値基づいてローパスフィルタ３２の時定数により定まる遮断周波数を調整する。ここで、調整の基準であるが、実施形態１の説明でも述べたように、例えば、かかる機械的結合剛性が高い場合は遮断周波数も高くなるようにするとよく、これに比較してかかる機械的結合剛性が低い場合は遮断周波数も低くなるようにするとよい。
【００６０】
これにより、本実施形態では、推定した、ねじれ剛性等の機械的結合剛性に基づいて、遮断周波数を調整することができる。そのため、本実施形態では、遮断周波数の値が不適切であり、スレーブ軸の積分値をマスター軸の積分値に一致させるとねじれが生じてしまうことを防止することが可能となる、という効果を奏する。
【００６１】
以上、本発明の実施形態について説明をした。上述した実施形態は、本発明の好適な実施形態ではあるが、上記実施形態のみに本発明の範囲を限定するものではなく、本発明の要旨を逸脱しない範囲において種々の変更を施した形態での実施が可能である。
【００６２】
また、以上説明した各実施形態は、ハードウェア、ソフトウェア又はこれらの組合せにより実現することができる。ここで、ソフトウェアによって実現されるとは、コンピュータがプログラムを読み込んで実行することにより実現されることを意味する。ハードウェアで構成する場合、各実施形態の一部又は全部を、例えば、ＬＳＩ(Large Scale Integrated circuit)、ＡＳＩＣ(Application Specific Integrated Circuit)、ゲートアレイ、ＦＰＧＡ(Field Programmable Gate Array)等の集積回路（ＩＣ）で構成することができる。
【００６３】
また、各実施形態の一部又は全部をソフトウェアとハードウェアの組み合わせで構成する場合、フローチャートで示されるサーボ制御装置の動作の全部又は一部を記述したプログラムを記憶した、ハードディスク、ＲＯＭ等の記憶部、演算に必要なデータを記憶するＤＲＡＭ、ＣＰＵ、及び各部を接続するバスで構成されたコンピュータにおいて、演算に必要な情報をＤＲＡＭに記憶し、ＣＰＵで当該プログラムを動作させることで実現することができる。
プログラムは、様々なタイプのコンピュータ可読媒体（computer readable medium）を用いて格納され、コンピュータに供給することができる。コンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。コンピュータ可読媒体の例は、磁気記録媒体（例えば、フレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば、光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む
【符号の説明】
【００６４】
１  上位制御装置
２  位置制御部
２−１  第１位置制御部
２−２  第２位置制御部
３−１  第１電流制御部
３−２  第２電流制御部
３−Ｍ  第Ｍ電流制御部
４−１  第１電流増幅器
４−２  第２電流増幅器
４−Ｍ  第Ｍ電流増幅器
５−１  第１速度検出器
５−２  第２速度検出器
５−Ｍ  第Ｍ速度検出器
６−１  第１サーボモータ
６−２  第２サーボモータ
６−Ｍ  第Ｍサーボモータ
７−１  第１伝達機構
７−２  第２伝達機構
７−Ｍ  第Ｍ伝達機構
８  被駆動体
９  位置検出器
１１、２１、３１、Ｍ１  減算器
１２、２２、Ｍ２  積分器
１３、２３、Ｍ３  比例ゲインの増幅器
１４、２４、Ｍ４  積分ゲインの増幅器
１５、２５、Ｍ５、３３  加算器
３２  ローパスフィルタ
４０  剛性推定調整部
４１  電流指令切替器
１００、１０１、１０２  速度制御部
【図１】
0006392805.tif 000002
【図２】
0006392805.tif 000003
【図３】
0006392805.tif 000004
【図４】
0006392805.tif 000005
【図５】
0006392805.tif 000006
【図６】
0006392805.tif 000007
【図７】
0006392805.tif 000008
【図８】
0006392805.tif 000009