JP2010142020A - サーボ制御装置とサーボ制御方法 - Google Patents

Abstract

【課題】 実際の慣性モーメントが、慣性モーメント設定値からずれた場合でも、位置決め時間が延びることなく、振動も発生しないで位置決めすることができるサーボ制御装置とサーボ制御方法を提供する。
【解決手段】 規範速度と推定速度の偏差を基に、補正慣性モーメントを算出する慣性モーメント補正部（５０）と、補正慣性モーメントを慣性モーメント設定値に加算して制御用慣性モーメントを算出する加算部（２７）と、制御用慣性モーメントを使用して速度制御演算をする速度制御部（４）、トルクフィードフォワード演算をするトルクフィードフォワード部（３０）、オブザーバ演算をするオブザーバ（９）を備える。
【選択図】図１

Description

本発明は、慣性モーメントの設定値がずれた場合も安定且つ、性能を劣化させることなく位置決めが可能なサーボ制御装置に関する。

ロボットのアーム駆動や搬送機械などに適用されるサーボ制御装置は、慣性モーメントの変動が変動しても所定の応答性・精度が要求される。これらの要求に応えた従来のサーボ制御装置は、入力された指令を基に規範モデルの演算を行う規範モデル演算部と、モータ速度を推定するオブザーバを備え、前記規範モデルで演算されるモータ規範速度とオブザーバ推定速度の偏差にゲインを乗じた補正速度指令を速度指令に加算するとともに、前記補正速度指令を積分して位置指令に加算することにより、慣性モーメントの設定値が実際の値と異なる場合も不安定にならないように構成している（例えば、特許文献１参照）。

図６は、本発明のサーボ制御装置の構成図である。図において１は制御対象、２は制御対象の位置を検出する検出器を表す。３は位置制御部、４は速度制御部、５は電流制御部を表し、それぞれの部は比例あるいは比例積分制御を行う。６は微分演算部を表し、位置検出値を微分して速度フィードバックｖｆｂを算出する。実際のコントローラでは離散信号を扱うため、微分には差分近似を用いるのが一般的である。７は規範モデル演算部部を表し、位置指令ｒｅｆから、慣性モーメント設定値と実際の慣性モーメントが一致している時の理想的な応答をした際の速度である規範速度ｖｍｄｌを計算し出力する。９はオブザーバを表し、トルク指令ｔｒｅｆと位置検出値ｘをもとに速度推定値を算出する。１０は減算部を表し、規範速度信号ｖｍｄｌから推定速度を減じる処理を行う。８はゲイン乗算部を表し、規範速度信号ｖｍｄｌと推定速度の差にゲインＧを乗じて補正速度指令ｖｃｏｍｐを出力する。１１は演算部を表し補正速度指令ｖｃｏｍｐと位置制御部の出力である速度指令ｖｒｅｆを加算して新たな速度指令とし、新たな速度指令から速度フィードバックｖｆｂを減算する。１００は積分演算記を表し、速度ずれ補正信号ｖｃｏｍｐを積算する。積算された信号を位置指令に加算することで従来の制御装置は構成されている。
このように、従来のサーボ制御装置は、指令から算出された規範速度と推定速度の偏差に応じて速度および位置に補正をかけることで慣性モーメントのズレにロバストな制御装置を実現しているのである。
特開２００７−３０６７７９号公報（第５−１０頁、図１１）

従来のサーボ制御装置は、実際の慣性モーメントが、制御装置内部で使用する慣性モーメント設定値と大きくずれた場合も不安定になることは無いが、実際の慣性モーメントと慣性モーメント設定値が一致している場合と比較すると、位置決め時間が長くなったり、位置決め時に振動が発生したりするという問題があった。

本発明はこのような問題点に鑑みてなされたものであり、実際の慣性モーメントが、慣性モーメント設定値からずれた場合でも位置決め時間が延びることなく、振動も発生しないで位置決めすることができるサーボ制御装置とサーボ制御方法を提供することを目的とする。

上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、位置指令とモータ位置の偏差を小さくするよう制御し速度指令を算出する位置制御部と、速度指令とモータ速度の偏差が小さくなるように制御し算出した操作量ｕｒｅｆに制御用慣性モーメントを乗算しトルク指令を算出する速度制御部と、前記トルク指令に基づいてモータに電流を供給する電流制御部と、位置指令に基づいて規範速度を算出する規範モデル演算部と、前記トルク指令とモータ位置と前記制御用慣性モーメントに基づいて推定速度を算出するオブザーバと、前記規範速度と前記推定速度の偏差である規範推定速度偏差にゲインを乗じ補正速度指令を算出するゲイン乗算部と、前記速度指令と補正速度指令を加算し新たな速度指令を算出する演算部と、前記モータ位置を微分して前記モータ速度を算出する微分演算部と、を備えたサーボ制御装置において、前記規範推定速度偏差を基に、補正慣性モーメントを算出する慣性モーメント補正部と、前記補正慣性モーメントと慣性モーメント設定値を加算して前記制御用慣性モーメントを算出する第１加算部と、を備えることを特徴とするものである。
請求項２に記載の発明は、位置指令とモータ位置の偏差を小さくするよう制御し速度指令を算出する位置制御部と、速度指令とモータ速度の偏差が小さくなるように制御し算出した操作量ｕｒｅｆに制御用慣性モーメントを乗算しトルク指令を算出する速度制御部と、前記トルク指令に基づいてモータに電流を供給する電流制御部と、位置指令に基づいて規範速度を算出する規範モデル演算部と、前記トルク指令とモータ位置と前記制御用慣性モーメントに基づいて推定速度を算出するオブザーバと、前記規範速度と前記推定速度の偏差である規範推定速度偏差にゲインを乗じ補正速度指令を算出するゲイン乗算部と、前記速度指令と補正速度指令を加算し新たな速度指令を算出する演算部と、前記モータ位置を微分して前記モータ速度を算出する微分演算部と、を備えたサーボ制御装置において、前記規範推定速度偏差を基に、補正慣性モーメントを算出する慣性モーメント補正部と、前記補正慣性モーメントと慣性モーメント設定値を加算してフィードフォワード用慣性モーメントを算出する第２加算部と、記前記位置指令を２回微分して算出した加速度指令に前記フィードフォワード用慣性モーメントを乗算しトルクフィードフォワード指令を算出するトルクフィードフォワード部と、前記トルクフィードフォワード指令を前記トルク指令に加算し新たなトルク指令を算出する第３加算部と、を備えることを特徴とするものである。
請求項３に記載の発明は、請求項１または２記載のモータ制御装置において、前記補正速度指令を積分し補正位置指令を算出する第１積分演算部と、前記補正位置指令と位置指令を加算して新たな位置指令とする第５加算部と、前記位置指令と前記モータ位置に基づいて前記速度指令を算出することを特徴とするものである。
請求項４に記載の発明は、請求項１または２記載のサーボ制御装置において、前記慣性モーメント補正部は、前記規範推定速度偏差を積分し規範推定速度偏差積分値を算出する第２積分演算部と前記規範推定速度偏差積分値に不感帯を持たせた第２規範推定速度偏差積分値を算出する不感帯演算部と、前記第２規範推定速度偏差積分値の絶対値をとって規範推定速度偏差積分絶対値を演算する絶対値演算部と、前記規範推定速度偏差積分絶対値にゲインを乗じ第２規範推定速度偏差積分絶対値を算出するゲイン演算部と、前記第２規範推定速度偏差積分絶対値を積算して補正慣性モーメントを算出する積算部を備ることを特徴とするものである。
請求項５に記載の発明は、請求項４記載のサーボ制御装置において、前記規範推定速度偏差積分値と前記補正慣性モーメントを、位置決め完了ごとに０にリセットすることを特徴とするものである。
請求項６に記載の発明は、位置指令とモータ位置の偏差を小さくするよう制御し速度指令を算出する位置制御部と、速度指令とモータ速度の偏差が小さくなるように制御し算出した操作量ｕｒｅｆに制御用慣性モーメントを乗算しトルク指令を算出する速度制御部と、前記トルク指令に基づいてモータに電流を供給する電流制御部と、位置指令に基づいて規範速度を算出する規範モデル演算部と、前記トルク指令とモータ位置と前記制御用慣性モーメントに基づいて推定速度を算出するオブザーバと、前記規範速度と前記推定速度の偏差にゲインを乗じ補正速度指令を算出するゲイン乗算部と、前記速度指令と補正速度指令を加算し新たな速度指令を算出する演算部と、前記モータ位置を微分して前記モータ速度を算出する微分演算部と、を備えたサーボ制御装置のサーボ制御方法において、前記規範速度と推定速度の偏差である規範推定速度偏差を積分して規範推定速度偏差積分値を算出し、前記規範推定速度偏差積分値に不感帯を持たせた第２軌範推定速度偏差積分値を算出し、前記第２規範推定速度偏差積分値の絶対値をとって規範推定偏差積分絶対値を演算し、前記規範推定偏差積分絶対値にゲインを乗じて第２規範推定速度偏差積分絶対値を算出し、前記第２規範推定速度偏差積分絶対値を積算して補正慣性モーメントを算出し、前記補正慣性モーメントを慣性モーメント設定値に加算し制御用慣性モーメントを算出するという手順で処理することを特徴とするものである。
請求項７記載の発明は、位置指令とモータ位置の偏差を小さくするよう制御し速度指令を算出する位置制御部と、速度指令とモータ速度の偏差が小さくなるように制御し算出した操作量ｕｒｅｆに制御用慣性モーメントを乗算しトルク指令を算出する速度制御部と、前記トルク指令に基づいてモータに電流を供給する電流制御部と、位置指令に基づいて規範速度を算出する規範モデル演算部と、前記トルク指令とモータ位置と前記制御用慣性モーメントに基づいて推定速度を算出するオブザーバと、前記規範速度と前記推定速度の偏差にゲインを乗じ補正速度指令を算出するゲイン乗算部と、前記速度指令と補正速度指令を加算し新たな速度指令を算出する演算部と、前記モータ位置を微分して前記モータ速度を算出する微分演算部と、を備えたサーボ制御装置のサーボ制御方法において、前記規範速度と推定速度の偏差である規範推定速度偏差を積分して規範推定速度偏差積分値を算出し、前記規範推定速度偏差積分値に不感帯を持たせた第２軌範推定速度偏差積分値を算出し、前記第２規範推定速度偏差積分値の絶対値をとって規範推定偏差積分絶対値を演算し、前記規範推定偏差積分絶対値にゲインを乗じて第２規範推定速度偏差積分絶対値を算出し、前記第２規範推定速度偏差積分絶対値を積算して補正慣性モーメントを算出し、前記補正慣性モーメントを慣性モーメント設定値に加算しフィードフォワード用慣性モーメントを算出し、位置指令を２回微分した加速度指令に前記フィードフォワード用慣性モーメントを乗じてトルクフィードフォワート指令を算出し、前記トルク指令に前記トルクフィードフォワード指令を加算して新たなトルク指令を算出することを特徴とするものである。
請求項８記載の発明は、請求項６または7記載のサーボ制御装置のサーボ制御方法において、前記規範推定速度偏差積分値と前記補正慣性モーメントを、位置決め完了ごとに０にリセットすることを特徴とするものである。

請求項１、３、６、７に記載の発明によると、予め計算している規範速度と推定速度の偏差をもとに補正慣性モーメントを自動的に算出し、制御装置内で使用している慣性モーメント設定値を補正することで、制御装置内で使用する慣性モーメントと実際の制御対象の慣性モーメントの誤差が小さくなり、位置決め時間が延びたり、振動が発生するという問題が解決される。
また、請求項２に記載の発明によると、特に、トルクフィードフォワード指令の計算で使用している慣性モーメントのみを補正するため、フィードバック制御部はもともとの安定なままなので、万が一、補正慣性モーメントが大きな誤差を持った場合も、動作が若干指令に対してずれるものの発振などせず、安全なサーボ制御装置を実現できる。
また、請求項４に記載の発明によると、前記規範速度と推定速度の偏差を積分することで、元の偏差に対して補正の感度を鈍くすることができ、積分された信号がある閾値以下の時は０を出力する不感帯演算を行うことで、慣性モーメントがほぼ補正された場合にノイズや外乱等の影響で、さらに誤補正してしまうのを避けることができ、前記不感帯演算部の出力の絶対値を演算することで、実機が振動的に動作し、規範モデルとの偏差が正負に出力されても、常に補正信号を大きくすることで即応性を上げることができ、絶対値演算部の出力にゲインを乗じることで、補正信号の収束の度合いを調整でき、ゲイン演算部の出力を積算することで、規範モデルとの偏差が小さくなり不感帯の幅に収まるまで、学習的に補正量を収束させることがき、結果として、すばやく制御装置内で使用する慣性モーメントと実際の制御対象の慣性モーメントの誤差が小さくなり、位置決め時間が延びたり、振動が発生するという問題が解決される。
また、請求項５および請求項８に記載の発明によると、前記慣性モーメント補正部内部の、積分演算部および積算部、位置決め完了ごとに０にリセットすることで、動作中に予期せぬ外乱等で補正慣性モーメントが大きな誤差を持った場合も、次の動作の時にはその影響がなくなるため、外乱による誤差の影響を次の動作に影響させずに、常に良好な動作を実現できる。

以下、本発明の実施の形態について図を参照して説明する。

図１は、本発明のサーボ制御装置のブロック図である。
図１において１は制御対象であり、２は制御対象の位置を検出する検出器を表す。３は位置制御部、４は速度制御部、５は電流制御部を表し、それぞれの部は比例あるいは比例積分制御を行う。６は微分演算部を表し、位置検出値を微分してモータ速度である速度フィードバックｖｆｂを算出する。実際のコントローラでは離散値を扱うため、微分には差分近似を用いるのが一般的である。７は規範モデル演算部を表し、位置指令ｒｅｆから、慣性モーメント設定値と実際の慣性モーメントが一致している時の理想的な応答をした際の速度である規範速度信号ｖｍｄｌを計算し出力する。９はオブザーバを表し、トルク指令ｔｒｅｆとモータ位置の位置検出値ｘをもとに推定速度ｖｈａｔを算出する。１０は減算部を表し、規範速度信号ｖｍｄｌから推定速度を減じる処理を行う。８はゲイン乗算部を表し、規範速度信号ｖｍｄｌと推定速度の差にゲインＧを乗じて補正速度指令ｖｃｏｍｐを算出する。１１は演算部で補正速度指令ｖｃｏｍｐと位置制御部が算出する速度指令ｖｒｅｆを加算して新たな速度指令とし、さらに速度フィードバック信号ｖｆｂを減算する。１００は積分演算部を表し、補正速度指令ｖｃｏｍｐを積算し、補正位置指令を算出する。１２の演算部は位置指令と補正位置指令を加算し新たな位置指令を算出する。

５０は慣性モーメント補正部を表し、規範速度ｖｍｄｌと推定速度ｖｈａｔの偏差に基づいて補正慣性モーメントＪｃｏｍｐを算出する。２２は規範速度と推定速度の偏差の規範推定速度偏差を積分し規範推定速度偏差積分値を算出する積分演算部、２３は規範推定速度偏差積分値がある閾値以下の時は０として不感帯をもたせた第２規範推定速度偏差積分値を算出する不感帯演算部、２４は第２規範推定速度偏差積分値の絶対値をとり規範推定速度偏差積分絶対値を演算する絶対値演算部、２５は規範推定速度偏差積分絶対値にゲインを乗じて第２規範推定速度偏差積分絶対値を算出するゲイン演算部、２６は第２規範推定速度偏差積分絶対値を積算し補正慣性モーメントＪｃｏｍｐを算出する積算部である。補正慣性モーメントＪｃｏｍｐは、速度制御部４およびオブザーバ９で使用される慣性モーメント設定値Ｊｎに加算され制御用慣性モーメントになるよう本実施例のサーボ制御装置は構成されている。

本発明が従来技術と異なる部分は、５０の慣性モーメント補正部を有し、慣性モーメント補正部の出力である補正慣性モーメントＪｃｏｍｐを、速度制御部４およびオブザーバ９で使用される慣性モーメント設定値Ｊｎに加算し制御用慣性モーメントにするようにしたところである。

次に、各部ごとの具体的な処理方法について述べる。

以下、サンプリング時間はＴｓ、今回のサンプリングをｋ番目、ｎ回前のサンプリングをｋ−ｎと定義し、変数ｓのｋ−ｎ番目のサンプリング時の値をｓ（ｋ−ｎ）と表記することにする。

まず、位置制御部では、式（１）に表すように位置指令ｒｅｆ（ｋ）と位置検出値ｘ（ｋ）の差に後述する補正位置指令ｘｃｏｍｐ（ｋ）を加算した信号に位置制御ゲインＫｐを乗じる計算を行い速度指令ｖｒｅｆ（ｋ）を出力する。
ｖｒｅｆ（ｋ）＝Ｋｐ・｛ｒｅｆ（ｋ）＋ｘｃｏｍｐ（ｋ）−ｘ（ｋ）｝・・・（１）

次に、速度制御部では、式（２）に表すように、速度指令ｖｒｅｆ（ｋ）と後で説明する補正速度指令ｖｃｏｍｐを加算して新たな速度指令とし、さらに速度フィードバックｖｆｂ（ｋ）を減算して速度偏差を算出する。次に速度偏差に速度制御遮断周波数である速度制御比例ゲインＫｖを乗じｓｒｅｆ（ｋ）を算出する。
ｓｒｅｆ（ｋ）＝Ｋｖ・｛ｖｃｏｍｐ（ｋ）＋ｖｒｅｆ（ｋ）−ｖｆｂ（ｋ）｝ ・・（２）
ここで、ｖｆｂ（ｋ）はモータ位置である位置検出値ｘ（ｋ）の差分近似による擬似微分で求めても良い。

次に、式（３）のように、加速度指令ｓｒｅｆ（ｋ）に積分制御ゲインＫｉおよびサンプリング時間Ｔｓを乗じ、前回値に加算することにより積分演算を行いｓｉ（ｋ）を算出する。
ｓｉ（ｋ）＝ｓｉ（ｋ−１）＋Ｔｓ・Ｋｉ・ｓｒｅｆ（ｋ） ・・・（３）

次に、式（４）のように、ｓｒｅｆ（ｋ）とｓｉ（ｋ）を加算し、慣性モーメント設定値である慣性モーメントノミナル値Ｊｎを乗算することでトルク指令ｔｒｅｆ（ｋ）を算出する。
ｔｒｅｆ（ｋ）＝Ｊｎ・（ｓｒｅｆ（ｋ）＋ｓｉ（ｋ）） ・・・（４）

次に、電流制御部では、トルク指令ｔｒｅｆ（ｋ）を入力し、単位変換および制御演算を行い電流値Ｉ（ｋ）を計算する。ここで電流制御部内部の処理は実際には交流を直流に変換するなど複雑な処理が行われるが、電流制御部内部の処理方法は本発明に全く関係がなく、どのような処理をしても良いためここでは説明を省略する。

次に、規範モデル演算部の処理を説明する。規範モデル演算部では制御対象のモデル以外は実際の位置制御部と速度制御部と同様に式（５）〜式（９）の処理を行う。
ｖｒｅｆ（ｋ）＝Ｋｐ・｛ｒｅｆ（ｋ）−ｘｍｄｌ（ｋ）｝ ・・・（５）
ｖｍｆｂ（ｋ）＝｛ｘｍｄｌ（ｋ）−ｘｍｄｌ（ｋ−１）｝／Ｔｓ ・・・（６）
ｓｒｅｆｍ（ｋ）＝Ｋｖ・｛ｖｍｒｅｆ（ｋ）−ｖｍｆｂ（ｋ）｝ ・・・（７）
ｓｉｍ（ｋ）＝ｓｉｍ（ｋ−１）＋Ｔｓ・Ｋｉ・ｓｒｅｆｍ（ｋ） ・・・（８）
ｔｒｅｆｍ（ｋ）＝Ｊｎ・（ｓｒｅｆｍ（ｋ）＋ｓｉｍ（ｋ）） ・・・（９）

そして、制御対象のモデルは１／（Ｊｎ・ｓ^２）とし、式（１０）、（１１）の演算を行うことで制御対象モデルの規範速度ｖｍｄｌと規範位置ｘｍｄｌを算出する。
ｖｍｄｌ（ｋ）＝ｖｍｄｌ（ｋ−１）＋Ｔｓ・ｔｒｅｆｍ（ｋ）／Ｊｎ・・・（１０）
ｘｍｄｌ（ｋ）＝ｘｍｄｌ（ｋ−１）＋Ｔｓ・ｖｍｄｌ（ｋ） ・・・（１１）
そして、規範モデル演算部は式（１０）で計算される規範速度ｖｍｄｌ（ｋ）を出力する。

ここで実際には制御対象モデルのイナーシャを慣性モーメント設定値Ｊｎとしているため、式（９）と式（１０）の計算からＪｎの乗算を省略してもよい。

また、７の規範モデル演算部では、単純にフィルタの処理のみを行ってもよい。
例えば、位置ループ比例ゲインＫｐの逆数を時定数とする一次のローパスフィルタに微分を組み合わせた式（１２）のようなフィルタ処理を行い、規範速度ｖｍｄｌを算出すればよい。
ｖｍｄｌ＝（Ｋｐ／（ｓ＋Ｋｐ）・ｓ）ｒｅｆ ・・・（１２）
ここでｓはラプラス演算子を表す。

あるいは、規範モデル内の演算である、位置指令ｒｅｆ（ｋ）からｖｍｄｌ（ｋ）を、そのまま伝達関数で表した式（１３）のようなフィルタ計算をしてもよい。
ｖｍｄｌ ＝ｓ・（Ｋｐ・Ｋｖ・ｓ＋Ｋｐ・Ｋｖ・Ｋｉ）／
（ｓ^３＋Ｋｖ・ｓ^２＋Ｋｖ（Ｋｐ＋Ｋｉ）・ｓ＋Ｋｐ・Ｋｖ・Ｋｉ）ｒｅｆ・・（１３）

次に、オブザーバ９の処理を説明する。ここで用いるオブザーバはどのような構成のものでも問題なく利用できるが、例えば式（１４）に示すようなオブザーバを用いれば良い。

ここで、式（１４）中の各変数はそれぞれ、ｘｈａｔ：位置推定値、ｖｈａｔ：速度推定値、ｄｈａｔ：外乱推定値、Ｔｓ：制御サンプリング周期、Ｊｎ：慣性モーメントノミナル値、ｋ：サンプリング（ｋ＝１、２、３・・・）である。また、Ｌ１、Ｌ２、Ｌ３はオブザーバゲインであり、推定誤差に対する収束速度を決定する変数である。これらの設定は、推定値の信号が汚くならない程度に、大きめの値を設定しておけばよい。

式（１４）から分かるようにオブザーバを使用すると、今回のトルク指令ｔｒｅｆ（ｋ）からｋ＋１番目の推定値が計算されるため、モータ速度ｖｆｂに対して推定速度ｖｈａｔは位相が１サンプリング進んだ信号となり、遅れを小さくすることでゲインＧの値を大きくすることができ、結果として慣性モーメントのずれや摩擦等の外乱があった場合の性能劣化をさらに改善することができるという利点がある。

次に、式（１５）のように、補正速度指令ｖｃｏｍｐは規範速度指令ｖｍｄｌと推定速度ｖｈａｔの差にゲインＧを乗じて求める。
ｖｃｏｍｐ（ｋ）＝Ｇ・｛ｖｍｄｌ（ｋ）−ｖｈａｔ（ｋ）｝・・・（１５）
ここで計算に用いたゲインＧの値は固定値でもよいが、速度ループゲインＫｖの値に応じて変更してもよい。例えば、Ｇを式（１６）のようにＫｖの関数としておきＫｖの値によって変更するようにすればよい。
Ｇ ＝ α／Ｋｖ ・・・（１６）
式（１６）のαは任意の定数でよいが、例えば５０００という値にしておけばよい。
こうすることで、制御調整時にＫｖを調整するだけで最適なＧの値が自動的に決定され、調整が簡単になる。

次に、積分演算部１００にて式（１７）のように補正速度指令ｖｃｏｍｐを積分し、補正位置指令ｘｃｏｍｐを算出する。
ｘｃｏｍｐ（ｋ）＝ｘｃｏｍｐ（ｋ−１）＋Ｔｓ・ｖｃｏｍｐ（ｋ）・・・（１７）
次に、演算部１２にて式（１８）の演算を行い、位置偏差ｅｒｒを求め、３の位置制御部への入力とする。
ｅｒｒ（ｋ）＝ｒｅｆ（ｋ）＋ｘｃｏｍｐ（ｋ）−ｘ（ｋ） ・・・（１８）

このように、位置指令ｒｅｆから規範となる規範速度ｖｍｄｌを算出し、規範速度ｖｍｄｌと推定速度ｖｈａｔの差にゲインを乗じた信号を補正速度指令ｖｃｏｍｐとし速度指令に加え新たな速度指令とし、ｖｃｏｍｐを積算した補正位置指令ｘｃｏｍｐを位置指令に加えて新たな位置指令とし、慣性モーメントＪのずれや摩擦等の外乱があった場合も、その影響少なくすることができ、結果的に、位置応答の劣化を最小限に押さえることができる。また、慣性モーメント設定値を設定しなくても、ゲインＧを十分大きな値にしておけば、モータ単体の慣性モーメントの数百倍の負荷が付いた場合も制御系が不安定にならないため安全なサーボ制御装置を実現することができる。

さらに、本発明では慣性モーメント補正部を有する。その処理の詳細を以下に説明する。
まず、２２の積分演算部では式（１９）に示すように、規範速度指令信号ｖｍｄｌと速度推定値ｖｈａｔの差の信号を積分し、信号ｄ１を出力する。
ｄ１（ｋ）＝ｄ１（ｋ−１）＋Ｔｓ・（ｖｍｄｌ（ｋ）−ｖｈａｔ（ｋ））・・・（１９）
次に２３の不感帯演算部では、式（２０）に示すような場合わけを行い信号ｄ２を出力する。
ｄ１（ｋ）≧ｄｓｅｔのとき ｄ２（ｋ）＝ｄ１
ｄ１（ｋ）＜ｄｓｅｔのとき ｄ２（ｋ）＝０ ・・・（２０）
ここでｄｓｅｔは予め設定した不感帯の閾値である。ｄｓｅｔの値は、慣性モーメントが真値に近づいた時に補正慣性モーメントを積算するのを停止する指針となる。また、慣性モーメントの誤差以外のノイズや外乱に対して、補正慣性モーメントが誤差を含むのを避ける効果もある。値は、予め試行錯誤のうえ決定すればよい。

次に２４の絶対値演算部部では、ｄ２の絶対値を求めｄ３として出力する。
ｄ３（ｋ）＝ＡＢＳ（ｄ２（ｋ）） ・・・（２１）
ここでＡＢＳはｄ２（ｋ）の絶対値をとることを意味する。
次に２５のゲイン演算部ではｄ３にゲインＧａを乗じてｄ４を出力する。
ｄ４（ｋ）＝ｄ３（ｋ）・Ｇａ ・・・（２２）

ゲインＧａの値は、補正信号の収束の度合いを調整するパラメータであり、システムが不安定にならない慣性モーメントの誤差の許容範囲であるロバスト安定範囲内で不安定にならないような最大の値を設定しておけばよい。
最後に２６の積算部でｄ４を積算し補正慣性モーメントＪｃｏｍｐを算出する。
Ｊｃｏｍｐ（ｋ）＝Ｊｃｏｍｐ（ｋ−１）＋ｄ４（ｋ） ・・・（２３）
ここまでが慣性モーメント補正部５０内部の処理の説明である。

次に、算出された補正慣性モーメントＪｃｏｍｐをもとに、制御装置内で使用する慣性モーメント設定値Ｊｎを補正する。速度制御部内の処理としては、具体的には図２に示すように、第１加算部２７で慣性モーメント設定値Ｊｎに補正慣性モーメントＪｃｏｍｐを加算し、制御用慣性モーメントを算出する。式に表すと式（２４）のようになる。
ｕｒｅｆ（ｋ）＝ｓｒｅｆ（ｋ）＋ｓｉ（ｋ）
ｔｒｅｆ（ｋ）＝ｕｒｅｆ（ｋ）・（Ｊｎ＋Ｊｃｏｍｐ（ｋ）） ・・・（２４）
また、オブザーバ９内部で使用する慣性モーメント設定値Ｊｎにも補正慣性モーメントＪｃｏｍｐを加算する。そして、加算された慣性モーメント設定値Ｊｎを新たなＪｎ制御用慣性モーメントとして式（１４）に示すオブザーバ演算を行えば良い。

このように、位置指令ｒｅｆから規範速度ｖｍｄｌを算出し、規範速度ｖｍｄｌと推定速度推定ｖｈａｔの差から補正慣性モーメントを算出し、速度制御部およびオブザーバで使用する慣性モーメント設定値Ｊｎに加算することで、慣性モーメントのずれに対して安定性を保ったまま、制御部で使用する制御用慣性モーメントの値を真値に近づけることができるため、位置決め動作時に位置決め時間が延びたり、振動が発生するという問題が解決されるのである。

図３は第２実施例の構成を示す図である。本実施例が実施例１と異なる部分は、フィードフォワード制御を行っている部分である。図３の３０はフィードフォワード演算部を示し、位置フィードフォワード指令ｘｆｆと速度フィードフォワード指令ｖｆｆとトルクフィードフォワード指令ｔｆｆを作成する。
フィードフォワード指令の作成方法はどのようなものでもよく、例えば、指令をそのまま位置フィードフォワード指令ｘｆｆとし、位置指令を一回微分した信号を速度フィードフォワード指令ｖｆｆとし、位置指令を２階微分した加速度指令ｕｆｆに慣性モーメント設定値Ｊｎを乗じた信号をトルクフィードフォワード指令ｔｆｆとしてもよい。
速度フィードフォワード指令ｖｆｆは演算部１１で速度指令と加算され新たな速度指令となり、トルクフィードフォワード指令ｔｆｆは第３加算器でトルク指令と加算され新たなトルク指令をなる。

次に、実施例１の式（１）から式（４）を、式（２５）および式（２７）のように変更する。
ｖｒｅｆ（ｋ）＝Ｋｐ・｛ｘｆｆ（ｋ）＋ｘｃｏｍｐ（ｋ）−ｘ（ｋ）｝・・・（２５）
ｓｒｅｆ（ｋ）＝Ｋｖ・｛ｖｆｆ（ｋ）＋ｖｃｏｍｐ（ｋ）
＋ｖｒｅｆ（ｋ）−ｖｆｂ（ｋ）｝ ・・・（２６）
ｔｒｅｆ（ｋ）＝ｔｆｆ（ｋ）＋Ｊｎ・（ｓｒｅｆ（ｋ）＋ｓｉ（ｋ））・・・（２７）

この場合７の規範モデル演算部の処理も基本的に実際の制御部と同様の処理を行う必要があるため、実際の制御部と同様に式（５）から式（９）の処理を式（２８）および式（３１）に変更する。
ｖｒｅｆ（ｋ）＝Ｋｐ・｛ｘｆｆ（ｋ）−ｘｍｄｌ（ｋ）｝ ・・・（２８）
ｓｒｅｆｍ（ｋ）＝Ｋｖ・｛ｖｆｆ（ｋ）＋ｖｍｒｅｆ（ｋ）−ｖｍｆｂ（ｋ）｝
・・・（２９）
ｓｉｍ（ｋ）＝ｓｉｍ（ｋ−１）＋Ｔｓ・Ｋｉ・ｓｒｅｆｍ（ｋ）・・・（３０）
ｔｒｅｆｍ（ｋ）＝ｔｆｆ（ｋ）＋Ｊｎ・（ｓｒｅｆｍ（ｋ）＋ｓｉｍ（ｋ））・（３１）

また、規範モデルの応答はフィードフォワード指令通りに動作するため、式（２８）〜式（３１）の演算は行わずに、速度フィードフォワード指令ｖｆｆを規範速度指令信号ｖｍｄｌとして使用しても良い。
次に、実施例１で説明した５０の慣性モーメント補正部の補正慣性モーメントＪｃｏｍｐを算出し、第２加算部２８で慣性モーメント設定値に加算し、フィードフォワート用慣性モーメントを算出する。図４にその場合の処理の一例を示す。
トルクフィードフォワード指令ｔｆｆは式（３２）で演算される。
ｔｆｆ（ｋ）＝（Ｊｎ＋Ｊｃｏｍｐ）・ｕｆｆ（ｋ）・・・（３２）
ここでｕｆｆ（ｋ）は位置指令ｒｅｆ（ｋ）を２回微分した値である。

このようにフィードフォワード制御で使用する慣性モーメントのみを補正することにより，モータ単体の慣性モーメントの数百倍の負荷が付いた場合も制御系が不安定にならず、且つ、フィードフォワードで使用する制御用慣性モーメントは真値に近づくため、位置決め応答は、慣性モーメント設定値が実際の慣性モーメントと一致している場合とほぼ同様の動きをし、結果として、安全かつ高性能な位置決め応答を実現するサーボ制御装置を構成することができる。

図５は第３実施例の構成を示す図である。図５中２７は位置決め完了信号発生部を表し、指令払い出し後、位置決めが完了し、予め設定した偏差以内になった時に位置決め完了信号を発生する。位置決め完了信号が発生された時、２６の積算部ならびに、２２の積分演算部の値を０にクリアする。その他の処理は実施例１と同じである。また、これと同様に、本処理を実施例２と組み合わせても良い。

このように、位置決め完了時に毎回、積算部および積分演算部の値をクリアすることで、動作中に予期せぬ外乱等御用慣性モーメントが大きな誤差を持った場合も、次の動作の時にはその影響がなくなるため、外乱による誤差の影響を次の動作に影響させずに、常に良好な動作を実現できる。

本発明の第１実施例を示すサーボ制御装置のブロック図 本発明の第１実施例の速度制御部のブロック図 本発明の第２実施例を示すサーボ制御装置のブロック図 本発明の第２実施例のフィードフォワード演算部のブロック図 本発明の第３実施例を示すサーボ制御装置のブロック図 従来のサーボ制御装置のブロック図

符号の説明

１ 制御対象
２ 検出器
３ 位置制御部
４ 速度制御部
５ 電流制御部
６ 微分演算部
７ 規範モデル演算部
８ ゲイン乗算部
９ オブザーバ
１０ 減算部
１１ 演算部
１２ 演算部
１３ 第３加算部
２２ 積分演算部
２３ 不感帯演算部
２４ 絶対値演算部
２５ ゲイン演算部
２６ 積算部
２７ 第１加算部
２８ 第２加算部
２９ 位置決め完了信号発生部
３０ フィードフォワード部
５０ 慣性モーメント補正部
１００ 積分演算部

Claims (8)

1. 位置指令とモータ位置の偏差を小さくするよう制御し速度指令を算出する位置制御部と、速度指令とモータ速度の偏差が小さくなるように制御し算出した操作量ｕｒｅｆに制御用慣性モーメントを乗算しトルク指令を算出する速度制御部と、前記トルク指令に基づいてモータに電流を供給する電流制御部と、位置指令に基づいて規範速度を算出する規範モデル演算部と、前記トルク指令とモータ位置と前記制御用慣性モーメントに基づいて推定速度を算出するオブザーバと、前記規範速度と前記推定速度の偏差である規範推定速度偏差にゲインを乗じ補正速度指令を算出するゲイン乗算部と、前記速度指令と補正速度指令を加算し新たな速度指令を算出する演算部と、前記モータ位置を微分して前記モータ速度を算出する微分演算部と、を備えたサーボ制御装置において、
   前記規範推定速度偏差を基に、補正慣性モーメントを算出する慣性モーメント補正部と、
   前記補正慣性モーメントと慣性モーメント設定値を加算して前記制御用慣性モーメントを算出する第１加算部と、
   を備えることを特徴とするサーボ制御装置。
2. 位置指令とモータ位置の偏差を小さくするよう制御し速度指令を算出する位置制御部と速度指令とモータ速度の偏差が小さくなるように制御し算出した操作量ｕｒｅｆに制御用慣性モーメントを乗算しトルク指令を算出する速度制御部と、前記トルク指令に基づいてモータに電流を供給する電流制御部と、位置指令に基づいて規範速度を算出する規範モデル演算部と、前記トルク指令とモータ位置と前記制御用慣性モーメントに基づいて推定速度を算出するオブザーバと、前記規範速度と前記推定速度の偏差である規範推定速度偏差にゲインを乗じ補正速度指令を算出するゲイン乗算部と、前記速度指令と補正速度指令を加算し新たな速度指令を算出する演算部と、前記モータ位置を微分して前記モータ速度を算出する微分演算部と、を備えたサーボ制御装置において、
   前記規範推定速度偏差を基に、補正慣性モーメントを算出する慣性モーメント補正部と、
   前記補正慣性モーメントと慣性モーメント設定値を加算してフィードフォワード用慣性モーメントを算出する第２加算部と、
   前記位置指令を２回微分して算出した加速度指令に前記フィードフォワード用慣性モーメントを乗算しトルクフィードフォワード指令を算出するトルクフィードフォワード部と、
   前記トルクフィードフォワード指令を前記トルク指令に加算し新たなトルク指令を算出する第３加算部と、
   を備えることを特徴とするサーボ制御装置。
3. 前記補正速度指令を積分し補正位置指令を算出する第１積分演算部と、前記補正位置指令と位置指令を加算して新たな位置指令とする第５加算部と、前記位置指令と前記モータ位置に基づいて前記速度指令を算出することを特徴とする請求項１または２記載のモータ制御装置。
4. 前記慣性モーメント補正部は、前記規範推定速度偏差を積分し規範推定速度偏差積分値を算出する第２積分演算部と前記規範推定速度偏差積分値に不感帯を持たせた第２規範推定速度偏差積分値を算出する不感帯演算部と、前記第２規範推定速度偏差積分値の絶対値をとって規範推定速度偏差積分絶対値を演算する絶対値演算部と、前記規範推定速度偏差積分絶対値にゲインを乗じ第２規範推定速度偏差積分絶対値を算出するゲイン演算部と、前記第２規範推定速度偏差積分絶対値を積算して補正慣性モーメントを算出する積算部を備ることを特徴とする請求項１または２記載のサーボ制御装置。
5. 前記規範推定速度偏差積分値と前記補正慣性モーメントを、位置決め完了ごとに０にリセットすることを特徴とする請求項４記載のサーボ制御装置。
6. 位置指令とモータ位置の偏差を小さくするよう制御し速度指令を算出する位置制御部と、速度指令とモータ速度の偏差が小さくなるように制御し算出した操作量ｕｒｅｆに制御用慣性モーメントを乗算しトルク指令を算出する速度制御部と、前記トルク指令に基づいてモータに電流を供給する電流制御部と、位置指令に基づいて規範速度を算出する規範モデル演算部と、前記トルク指令とモータ位置と前記制御用慣性モーメントに基づいて推定速度を算出するオブザーバと、前記規範速度と前記推定速度の偏差にゲインを乗じ補正速度指令を算出するゲイン乗算部と、前記速度指令と補正速度指令を加算し新たな速度指令を算出する演算部と、前記モータ位置を微分して前記モータ速度を算出する微分演算部と、を備えたサーボ制御装置のサーボ制御方法において、
   前記規範速度と推定速度の偏差である規範推定速度偏差を積分して規範推定速度偏差積分値を算出し、
   前記規範推定速度偏差積分値に不感帯を持たせた第２軌範推定速度偏差積分値を算出し、
   前記第２規範推定速度偏差積分値の絶対値をとって規範推定偏差積分絶対値を演算し、
   前記規範推定偏差積分絶対値にゲインを乗じて第２規範推定速度偏差積分絶対値を算出し、
   前記第２規範推定速度偏差積分絶対値を積算して補正慣性モーメントを算出し、
   前記補正慣性モーメントを慣性モーメント設定値に加算し制御用慣性モーメントを算出するという手順で処理することを特徴とするサーボ制御装置のサーボ制御方法。
7. 位置指令とモータ位置の偏差を小さくするよう制御し速度指令を算出する位置制御部と、速度指令とモータ速度の偏差が小さくなるように制御し算出した操作量ｕｒｅｆに制御用慣性モーメントを乗算しトルク指令を算出する速度制御部と、前記トルク指令に基づいてモータに電流を供給する電流制御部と、位置指令に基づいて規範速度を算出する規範モデル演算部と、前記トルク指令とモータ位置と前記制御用慣性モーメントに基づいて推定速度を算出するオブザーバと、前記規範速度と前記推定速度の偏差にゲインを乗じ補正速度指令を算出するゲイン乗算部と、前記速度指令と補正速度指令を加算し新たな速度指令を算出する演算部と、前記モータ位置を微分して前記モータ速度を算出する微分演算部と、を備えたサーボ制御装置のサーボ制御方法において、
   前記規範速度と推定速度の偏差である規範推定速度偏差を積分して規範推定速度偏差積分値を算出し、
   前記規範推定速度偏差積分値に不感帯を持たせた第２軌範推定速度偏差積分値を算出し、
   前記第２規範推定速度偏差積分値の絶対値をとって規範推定偏差積分絶対値を演算し、
   前記規範推定偏差積分絶対値にゲインを乗じて第２規範推定速度偏差積分絶対値を算出し、
   前記第２規範推定速度偏差積分絶対値を積算して補正慣性モーメントを算出し、
   前記補正慣性モーメントを慣性モーメント設定値に加算しフィードフォワード用慣性モーメントを算出し、
   位置指令を２回微分した加速度指令に前記フィードフォワード用慣性モーメントを乗じてトルクフィードフォワート指令を算出し、前記トルク指令に前記トルクフィードフォワード指令を加算して新たなトルク指令を算出することを特徴とするモータ制御装置のモータ制御方法。
8. 前記規範推定速度偏差積分値と前記補正慣性モーメントを、位置決め完了ごとに０にリセットすることを特徴とする請求項６または7記載のサーボ制御装置のサーボ制御方法。