JP3975537B2 - Control device and control method - Google Patents

Description

……（１）
【０００７】
ＦＢ制御演算部１６は下記式２に規定される制御演算を行う。
【数１５】

……（２）
【０００８】
式１、２において、制御パラメータｋ_ｐは比例定数（比例利得または比例ゲイン）、制御パラメータｋ_ｄは微分定数（微分ゲイン）、制御パラメータｋ_ｉは積分定数（積分ゲイン）を示す。制御パラメータαはフィードフォワードゲイン（利得）を示し、制御パラメータβは速度フィードフォワードゲインを示す。記号Ｔはサンプリング周期を示し、変数（ｚ_−１）は一次遅れ要素を示す。したがって、この例では、ＦＦ制御演算部１２は比例（Ｐ）制御動作と微分（Ｄ）制御動作との組合せ演算を行う。ＦＢ制御演算部１６はＰＩＤ制御動作を行う。
【０００９】
図１に図解した制御装置の概略動作を述べる。ＦＦ制御演算部１２は目標位置信号Ｐ_ｒｅｆ（または目標角度信号Ａ_ｒｅｆ）に対して比例および微分（ＰＤ）制御演算を行う。加算回路１４が目標位置信号Ｐ_ｒｅｆ（または目標位置信号Ｐ_ｒｅｆ）に対するエンコーダ２００で検出した実際の位置検出信号（または角度検出信号）を減じて偏差を算出する。ＦＢ制御演算部１６がその偏差に対してＰＩＤ演算をする。加算回路１８は、ＦＦ制御演算部１２の演算結果とＦＢ制御演算部１６の演算結果とを加算する。以上の演算はディジタル的に（離散的に）行われる。その演算結果がＤ／Ａコンバータ２０においてアナログ信号に変換されてモータ駆動部（ドライバ）２を介してモータ１を駆動制御する。その結果がエンコーダ２００で検出されて、サンプラ３０を介して加算回路１４に印加される。この制御動作が所定のサンプリング周期で反復動作する。
【００１０】
【発明が解決しようとする課題】
図１に示すサーボ制御装置においては制御性能を向上させる制御パラメータ、たとえば、比例定数ｋ_Ｐ、微分定数ｋ_ｄ、積分定数ｋ_ｉ、α、βのみによって制御が行われているが、実際の駆動系においてはモータドライバ２およびモータ１などの駆動部のばね特性などにより共振特性を持つことが多い。この共振特性を無視して制御を行うと、共振特性により制御系の安定性が損なわれることがある。ばね特性については図３を参照して後述する。
【００１１】
一般に、制御性能の高い制御パラメータ（精密な制御を行うのに使用する感度の高い制御パラメータ）を用いると制御系の安定性が低くなり、上記共振特性により制御系の安定性が損なわれる傾向がある。逆に、制御系の安定性を保つためには感度の低い制御パラメータ（制御性能を低下させた制御パラメータ）で駆動系を制御せねばならない。そうすると充分な制御が行われない。このように、制御には制御パラメータの感度の高低によって二律背反する問題が内在している。
【００１２】
本発明は上述した制御に関する問題を克服するものであり、本発明の目的は制御対象の機構系などに「ばね要素」が存在してもそれを相殺する共振相殺フィルタを適用することにより、安定かつ精密な制御を可能とする制御装置と制御方法を提供することにある。
【００１３】
また本発明の目的は、上記共振相殺フィルタのパラメータを容易かつ適切に調整可能な共振相殺フィルタのパラメータ調整方法および調整装置を提供することにある。
【００１４】
【課題を解決するための手段】
上述の目的を達成するために、本発明では駆動系中に含まれるばね要素によって生じる共振特性を相殺するために２次進み要素および／または２次遅れ要素からなる共振相殺フィルタにより制御信号をフィルタリングする。
【００１５】
本発明の制御装置は、基本構成として、制御対象および／または該制御対象のアクチュエータにばね特性を有する制御系を離散時間方法で制御する制御装置において、目標指令信号と前記制御対象の位置または角度検出信号との差を求める偏差算出部と、該偏差算出部で算出した偏差に応じてフィードバック制御演算を行うフィードバック制御演算部と、該結果に対して前記ばね特性を相殺する演算を行う共振相殺フィルタ要素とを有する。
【００１６】
前記共振相殺フィルタ要素は、振幅極小値をもたらす２次進み要素：（ｓ−ｚ _fr ＋ｊ×ｚ _fi ）（ｓ−ｚ _fr −ｊ×ｚ _fi ）、または、振幅極大値をもたらす２次遅れ要素：１／（ｓ−ｐ _fr ＋ｊ×ｐ _fi ）（ｓ−ｐ _fr −ｊ×ｐ _fi ）、あるいは、両者を有する。
すなわち、前記共振相殺フィルタ要素は、（ｓ−ｚ _fr ＋ｊｚ _fi ）（ｓ−ｚ _fr −ｊｚ _fi ）で規定する振幅極小値をもたらす２次進み要素、および／または、〔１／（ｓ−ｐ _fr ＋ｊｐ _fi ）（ｓ−ｐ _fr −ｊｐ _fi ）〕で規定される振幅極大値をもたらす２次遅れ要素を有する。
前記２次進み要素および前記２次遅れ要素を規定する記号およびパラメータについて、ｓはラプラス演算子であり、ｊは虚数を表す演算子であり、（ｚ _fr ＋ｊｚ _fi ）、（ｚ _fr −ｊｚ _fi ）は、前記制御系の共振零点の座標（ｚ _fr ±ｊｚ _fi ）を示し、ｚ _fr は共振零点の実数部であり、ｚ _fi は共振零点の虚数部であり、（ｐ _fr ＋ｊｐ _fi ）、（ｐ _fr −ｊｐ _fi ）は、前記制御系の共振極の座標（ｐ _fr ±ｊｐ _fi ）を示し、ｐ _fr は共振極の実数部であり、ｐ _fi は共振極の虚数部である。
【００１７】
好適には、当該制御装置は、目標指令信号に応答してフィードフォワード制御演算を行うフィードフォワード制御演算部と、前記フィードフォワード制御演算部の演算結果と前記フィードバック制御演算部の演算結果を加算する加算部とを更に有し、
前記共振相殺フィルタ要素は、前記加算結果について、前記振幅極小値をもたらす２次進み要素、および／または、前記振幅極大値をもたらす２次遅れ要素についての前記フィルタ処理を行う。
【００１８】
また好適には、前記共振相殺フィルタ要素の前記振幅極小値をもたらす２次進み要素を示すフィルタ特性は、該共振相殺フィルタ要素の零点の座標：（ｚ _fr ±ｊｚ _fi ）と、前記制御対象および／またはアクチュエータの共振極の座標：（ｐ _fr ±ｊｐ _fi ）とを、（ｚ _fr ±ｊｚ _fi ）＝（ｐ _fr ±ｊｐ _fi ）と一致させた制御解析を行って得られ、
前記共振相殺フィルタ要素の前記振幅極大値をもたらす２次遅れ要素フィルタ特性は、前記共振相殺フィルタ要素の極の座標：（ｐ _fr ±ｊｐ _fi ）と前記制御対象および／またはアクチュエータの共振零点の座標：（ｚ _fr ±ｊｚ _fi ）とを、（ｐ _fr ±ｊｐ _fi ）＝（ｚ _fr ±ｊｚ _fi ）と一致させた制御解析を行って得られる。
（ｚ _fr ±ｊｚ _fi ）は前記共振相殺フィルタ要素の零点の座標であり、（ｐ _fr ±ｊｐ _fi ）は前記共振相殺フィルタ要素の極の座標である。
【００１９】
また本発明の制御装置は、前記制御解析で得られた前記共振相殺フィルタ要素の下記パラメータａ，ｂ，ｃ，ｄを前記共振相殺フィルタ要素に設定する共振相殺フィルタ用パラメータ設定部をさらに有する。
共振相殺フィルタ用パラメータ設定部は、前記ばね特性を相殺する、利得ｋｆ、パラメータａ，ｂ，ｃ，ｄ、一次遅れ要素Ｚ ^-1 （ＺはＺ変換演算子）、二次遅れ要素Ｚ ^-2 で表す、下記式で規定される前記共振相殺フィルタ要素のパラメータａ，ｂ，ｃ，ｄを求めて前記共振相殺フィルタ要素に設定する。
【００２０】
【数３】

【００２１】
好適には、前記共振相殺フィルタ用パラメータ設定部は、振幅極小周波数ｆ５ｆｚおよび極小値ダンピング係数ｄ_ｆｚから下記式を適用して前記共振相殺フィルタ要素の零点ｚ_ｆｒ±ｚ_ｆｉｊを求めて前記共振相殺フィルタ要素の振幅極小周波数および振幅極小値の鋭さを独立に指定する。
【００２２】
【数１６】

【００２３】
また好適には、前記共振相殺フィルタ用パラメータ設定部は、振幅極大周波数ｆ_ｆｐおよび極大値ダンピング係数ｄ_ｆｐから下記式を適用して前記共振相殺フィルタ要素の極ｐ_ｆｒ±ｐ_ｆｉｊを求め、前記共振相殺フィルタ要素の振幅極大周波数および振幅極大値の鋭さを独立に指定する。
【数１７】

【００２４】
さらに本発明によれば、制御対象および／または該制御対象のアクチュエータにばね特性を有する制御系を離散時間方法で制御するとき前記ばね特性を相殺する下記式で規定される共振相殺フィルタ処理を行う制御方法における前記共振相殺フィルタ要素のパラメータ設定方法であって、
【数１８】

前記共振相殺フィルタ要素の振幅極大周波数ｆ_ｆｐ、極大値ダンピング係数ｄ_ｆｐ、振幅極小周波数ｆ_ｆｚ、極小値ダンピング係数ｄ_ｆｚを入力し、 下記式の演算を行い前記共振相殺フィルタ要素の極の座標（ｐ_ｆｒ±ｐ_ｆｉｊ）および零点の座標（ｚ_ｆｒ±ｚ_ｆｉｊ）を算出し、
【数１９】

【数２０】

下記式の演算を行い前記共振相殺フィルタ要素の零点の座標（Ｚ_ｆｒ±Ｚ_ｆｉｊ）および極の座標（Ｐ_ｆｒ±Ｐ_ｆｉｊ）を求め、
【数２１】

【数２２】

下記式の演算を行いパラメータａ、ｂ、ｃ、ｄを算出し、
【数２３】

【数２４】

【数２５】

【数２６】

算出したパラメータａ、ｂ、ｃ、ｄを前記共振相殺フィルタ要素に設定する
共振相殺フィルタ用パラメータ設定方法が提供される。
【００２９】
【発明の実施の形態】
本発明の制御装置の実施の形態としての制御装置を添付図面を参照して述べる。
【００３０】
図２は本発明に係る制御装置の実施の形態としての、サーボモータを制御対象としたサーボ制御装置の構成図である。
【００３１】
図２に図解したサーボ制御装置の構成は図１に図解したサーボ制御装置に類似しているが、図２に図解した制御器（コントローラ）１００Ａの制御演算部１０Ａには共振相殺フィルタ要素１９が付加されており、制御器１００Ａには共振相殺フィルタ要素１９のパラメータを設定するための共振相殺フィルタ用パラメータ設定部４０が付加されている。
【００３２】
すなわち、図２に図解したサーボ制御装置は、制御対象としてのモータ１をモータ駆動部（ドライバ）２を介して駆動する制御器１００Ａとモータ１のロータの回転位置を検出するロータリィエンコーダ２００とを有する。
【００３３】
制御器１００Ａは、目標位置信号Ｐ_ｒｅｆ（または目標角度信号Ａ_ｒｅｆ）からフィードフォワード制御（正帰還制御、ＦＦ制御）演算を行うフィードフォワード（ＦＦ）制御演算部１２、第１の加算回路１４、フィードバック制御（負帰還制御、ＦＢ制御）演算を行うフィードバック（ＦＢ）制御演算部１６、第２の加算回路１８および共振相殺フィルタ要素１９を有する制御演算部１０Ａと、ディジタル／アナログ変換回路（Ｄ／Ａコンバータ）２０と、エンコーダ２００の検出信号をサンプリングして加算回路１４に印加するサンプラ３０と、共振相殺フィルタ用パラメータ設定部４０とを有する。制御器１００Ａもサンプラ３０において所定のサンプリング周期でサンプリングを行って制御する離散的な制御器であり、その制御内容はＦＦ制御演算部１２におけるフィードフォワード制御と、加算回路１４において算出した位置偏差＝（目標位置信号Ｐ_ｒｅｆ（または目標角度信号Ａ_ｒｅｆ）−エンコーダ２００の位置検出信号）に基づくＦＢ制御演算部１６におけるフィードバック制御とを組み合わせたものである。
【００３４】
ＦＦ制御演算部１２は式１に示した制御アルゴリズムに従ってフィードフォワード制御演算を行う。ＦＢ制御演算部１６は式２に示した制御アルゴリズムに従ってフィードバック制御演算を行う。この例では、ＦＦ制御演算部１２は比例（Ｐ）制御動作と微分（Ｄ）制御動作との組合せ演算を行い、ＦＢ制御演算部１６はＰＩＤ制御動作を行う。
【００３５】
共振相殺フィルタ要素１９は下記式３に示す演算を行う。
【数２９】

……（３）
【００３６】
共振相殺フィルタ要素１９の特性としては、通常、少なくとも２次遅れ要素を含むが、制御対象によっては２次進み要素を含む場合もある。本実施の形態においては、代表的な例示として、２次遅れ要素および２次進み要素の両者を含む場合を述べる。
【００３７】
図２に図解したサーボ制御装置の動作を述べる。
ＦＦ制御演算部１２は目標位置信号Ｐ_ｒｅｆ（または目標角度信号Ａ_ｒｅｆ）について式１に基づいて比例および微分（ＰＤ）制御演算を行う。加算回路１４が目標位置信号Ｐ_ｒｅｆ（または目標角度信号Ａ_ｒｅｆ）に対するエンコーダ２００で検出した実際の位置検出信号（または角度位置検出信号）を減じて偏差を算出する。ＦＢ制御演算部１６がその偏差に対して式２に基づいてＰＩＤ演算をする。加算回路１８はＦＦ制御演算部１２の演算結果とＦＢ制御演算部１６の演算結果とを加算する。共振相殺フィルタ要素１９は式３に基づいて演算をする。以上の演算はディジタル的に行われる。その演算結果がＤ／Ａコンバータ２０においてアナログ信号に変換されてモータ駆動部２を介してモータ１を駆動制御する。モータドライバ２はＤ／Ａコンバータ２０からの駆動信号に従ってモータ１に供給する電力を調整してモータの回転動作を制御する。モータ１の回転動作結果がエンコーダ２００で検出されて所定のサンプリング周期でサンプラ３０を介して加算回路１４に印加される。この制御動作が所定のサンプリング周期で反復されていく。
【００３８】
以下、共振相殺フィルタ要素１９および共振相殺フィルタ用パラメータ設定部４０について述べる。
【００３９】
共振相殺フィルタ要素１９
共振相殺フィルタ要素１９は、式３に示したように、２次遅れ要素、２次進み要素およびゲインｋ_ｆからなり、式３で規定された演算を行うことにより駆動系の共振極を相殺して制御系の安定性を向上させる。
なお駆動系の共振極については特開平８−１７１４０２号公報に詳述されている。
【００４０】
共振相殺フィルタ要素１９による演算が制御系の安定性を向上させる理由について述べる。
ばね要素を有する駆動系（モータ１およびモータドライバ２）を一般化すると、図３に図解のように、慣性１と慣性２とを有する２慣性系として表される。２つの慣性１、２がばね要素を介して連結されている。慣性１、２にはそれぞれ粘性１、２が付随している。図３に図解した慣性系の伝達関数Ｇ（ｓ）は一般に下記式で表すことができる。
【数３０】

……（４）
ただし、Ｊ_１は慣性１のイナーシャ定数（慣性定数）であり、
Ｊ_２は慣性２のイナーシャ定数（慣性定数）であり、
Ｋはばね要素のバネ定数であり、
Ｄ_１は慣性１の粘性摩擦係数であり、
Ｄ_２は慣性２の粘性摩擦係数であり、
ｓはラプラス演算子である。
【００４１】
この２慣性系の周波数特性を図４（Ａ）、（Ｂ）に示す。図４（Ａ）は振幅特性を示すグラフであり、図４（Ｂ）は位相特性を示すグラフである。図４（Ａ）、（Ｂ）はそれぞれ、ばね要素がある場合の特性曲線Ａ１、Ｐ１と、ばね要素がない場合の特性曲線Ａ０、Ｐ０を示す図である。ばね要素がない場合とは、ばね要素の代わりに理想的な剛体で連結されていると仮定した場合である。図４（Ａ）に図解した振幅特性について考察すると、ばね要素がある場合はばね要素がない場合に比べて、共振特性の共振極による振幅極大値、および共振特性に共振零点による振幅極小値が見られる。この振幅極大値の影響で高周領域での振幅が広い周波数範囲にわたって増大され、振幅特性が上昇する。このために駆動部からなる制御対象および制御装置で構成される制御系全体での開ループ伝達関数を考察すると利得余裕（ゲインマージン）が減少し、制御系が不安定になり、発振しやすくなる。
【００４２】
この課題を克服して上記駆動部に安定性の高い動作制御を行うため図２に図解した共振相殺フィルタ要素１９を用いる。以下、共振相殺フィルタ要素１９について詳述する。
【００４３】
駆動部の伝達関数Ｇ（ｓ）を示す式４は式５のように書き改めることができる。下記式において、ｓはラプラス演算子であり、ｊは虚数を表す演算子であり、（ｚ _r ＋ｊｚ _i ）、（ｚ _r −ｊｚ _i ）は、前記制御系の共振零点の座標（ｚ _r ±ｊｚ _i ）を示し、ｚ _r は共振零点の実数部であり、ｚ _i は共振零点の虚数部であり、（ｐ _r ＋ｊｐ _i ）、（ｐ _r −ｊｐ _i ）は、前記制御系の共振極の座標（ｐ _r ±ｊｐ _i ）を示し、ｐ _r は共振極の実数部であり、ｐ _i は共振極の虚数部である。
【数３１】

……（５）
【００４４】
式５を考察すると、１慣性系：１／ｓ（Ｊ’ｓ＋Ｄ’）に、２次進み要素：（ｓ−ｚ_ｒ＋ｚ_ｉｊ）（ｓ−ｚ_ｒ−ｚ_ｉｊ）、２次遅れ要素：１／（ｓ−ｐ_ｒ＋ｐ_ｉｊ）（ｓ−ｐ_ｒ−ｐ_ｉｊ）、および、ゲインＫ_ｆを掛け合わせたものと考えることができる。ただし、ゲインＫ_ｆは式５と式４の振幅周波数特性を同一とするような定数ゲインである。
【００４５】
図４（Ａ）の振幅極小値は２次進み要素：（ｓ−ｚ_ｒ＋ｚ_ｉｊ）（ｓ−ｚ_ｒ−ｚ_ｉｊ）によるものであり、振幅極大値は２次遅れ要素：１／（ｓ−ｐ_ｒ＋ｐ５ｉｊ）（ｓ−ｐ_ｒ−ｐ_ｉｊ）によるものである。したがって、図２に図解したサーボ制御装置の中に、振幅極小値をもたらす２次進み要素：（ｓ−ｚ_ｒ＋ｚ５ｉｊ）（ｓ−ｚ_ｒ−ｚ_ｉｊ）と、振幅極大値をもたらす２次遅れ要素：１／（ｓ−ｐ_ｒ＋ｐ_ｉｊ）（ｓ−ｐ_ｒ−ｐ_ｉｊ）を相殺する成分を挿入することが望ましい。その相殺成分を式６に示す。
【数３２】

……（６）
【００４６】
共振相殺フィルタ要素１９は基本的に式６の演算を行う。ただし、共振相殺フィルタ要素１９の零点：ｚ _ｆｒ ±ｚ_ｆｉｊと、駆動系の共振極：ｐ_ｒ±ｐ_ｉｊを一致させる、すなわちｚ_ｆｒ±ｚ_ｆｉｊ＝ｐ_ｒ±ｐ_ｉｊとし、また共振相殺フィルタ要素１９の極：ｐ_ｆｒ±ｐ_ｆｉｊと駆動系の共振零点：ｚ_ｒ±ｚ_ｉｊを一致させ、すなわち、ｐ_ｆｒ±ｐ_ｆｉｊ＝ｚ _ｆｒ ±ｚ _fi ｊとすることにより図３に図解した２慣性系の共振極および共振零点（図４（Ａ））は共振相殺フィルタ要素１９によって相殺されて、疑似的に１慣性系：１／ｓ（Ｊ’ｓ＋Ｄ’）と見なすことができる。その結果、駆動系の共振特性による周波数特性の振幅極大値が相殺され、１慣性系のような周波数特性となる（図４（Ａ）の曲線Ｐ０）。これにより、開ループ伝達関数でみたときのゲイン余裕の減少も救済されて、安定性の高い制御を行うことができる。このように、式３に示した特性を持つ共振相殺フィルタ要素１９を設けることにより、制御系の安定性が増し良好な制御が実現できる。
【００４７】
共振相殺フィルタ用パラメータ設定部４０
次いで、共振相殺フィルタ用パラメータ設定部４０について述べる。
図２に図解した共振相殺フィルタ用パラメータ設定部４０は、共振相殺フィルタ要素１９の振幅極大周波数ｆ_ｆｐおよび極大値ダンピング係数ｄ_ｆｐ、または共振相殺フィルタの振幅極小周波数ｆ_ｆｚおよび極小値ダンピング係数ｄ_ｆｚを指定し、共振相殺フィルタ要素１９における式３に現れるパラメータａ、ｂ、ｃ、ｄを出力する。
【００４８】
ダンピング係数について述べる。
共振相殺フィルタ要素１９の周波数特性は一般に図５（Ａ）、（Ｂ）のごとく図解される。図５（Ａ）は共振相殺フィルタ要素１９の振幅特性を示すグラフであり、図５（Ｂ）は位相特性を示すグラフである。図５（Ａ）に図解のごとく、１つの振幅極小値と１つの振幅極大値を持つ。共振相殺フィルタ要素１９において、振幅極小値を与える周波数ｆ_ｆｚおよび振幅極小値の鋭さを駆動系共振極の周波数ｆ_ｐおよび鋭さに一致させることにより駆動系共振極の影響を相殺することができる。また、共振相殺フィルタ要素１９の振幅極大値を与える振幅極大周波数ｆ_ｆｐおよび振幅極大値の鋭さを駆動系共振零点の周波数ｆ_ｚおよび鋭さに一致させることにより駆動系共振零点の影響を相殺することができる。しかし、共振相殺フィルタ要素１９の振幅極小値特性を与える零点：ｚ_ｆｒ±ｚ_ｆｉｊを求めるため、零点の実部ｚ_ｆｒもしくは虚部ｚ_ｆｉを変えると、振幅極小周波数ｆ_ｆｚおよび振幅極小値の鋭さが同時に変化してしまい、共振相殺フィルタ要素１９の適切な零点を求めるのは手間がかかる。また同様に、共振相殺フィルタ要素１９の極大値特性を与える極ｐ_ｆｒ±ｐ_ｆｉｊを求めるため、極の実部ｐ_ｆｒもしくは虚部ｐ_ｆｉを変えると、振幅極大周波数ｆ_ｆｐおよび振幅極大値の鋭さが同時に変化してしまい、共振相殺フィルタ要素１９の適切な極を求めるのは手間がかかる。
【００４９】
そこで本実施の形態においては、共振相殺フィルタ要素１９の振幅極小値の鋭さに相当する極小値ダンピング係数ｄ_ｆｚを式７、共振相殺フィルタ要素１の振幅極大値の鋭さに相当する極大値ダンピング係数ｄ_ｆｐを式８のように定める。
【数３３】

……（７）
【数３４】

……（８）
【００５０】
振幅極小周波数ｆ_ｆｚ（Ｈｚ）および振幅極大周波数ｆ_ｆｐ（Ｈｚ）はそれぞれ式９および式１０で与えられる。
【数３５】

……（９）
【数３６】

……（１０）
【００５１】
これにより共振相殺フィルタ要素１９の零点：ｚ_ｆｒ±ｚ_ｆｉｊは、上記した振幅極小周波数ｆ_ｆｚおよび上記極小値ダンピング係数ｄ_ｆｚから式１１として求められる。
【数３７】

……（１１）
【００５２】
極小値ダンピング係数ｄ_ｆｚ＞１の場合、零点は実軸上となり全く振動性をもたず共振の相殺には寄与しない。そこで、０＜ｄ_ｆｚ≦１の範囲で極小値ダンピング係数ｄ_ｆｚを設定することにする。極小値ダンピング係数ｄ_ｆｚを変化させた時の共振相殺フィルタ要素１９中の２次進み要素の周波数特性の変化の様子を図６（Ａ）、（Ｂ）に示す。
図６（Ａ）は振幅特性を示すグラフであり、図６（Ｂ）は位相特性を示すグラフである。
図６（Ａ）を参照すると、極小値ダンピング係数ｄ_ｆｚが大きくなると振幅極小周波数ｆ_ｆｚは変わらずに振幅極小値の負のピークがなだらかになる。すなわち、式７で示される極小値ダンピング係数ｄ_ｆｚは振幅極小値の鋭さのみに影響することがわかる。同様に、上記共振相殺フィルタの極ｐ_ｆｒ±ｐ_ｆｉｊは、振幅極大周波数ｆ_ｆｐおよび極大値ダンピング係数ｄ_ｆｐから式１１として求められる。
【数３８】

……（１２）
【００５３】
極大値ダンピング係数ｄ_ｆｐ＞１だと極は実軸上となり全く振動性をもたず共振の相殺には寄与しない。そこで本実施の形態においては、０＜ｄ_ｆｐ≦１の範囲で極大値ダンピング係数ｄ_ｆｐを設定することにする。
【００５４】
極大値ダンピング係数ｄ_ｆｐを変化させた時の共振相殺フィルタ要素１９中の２次遅れ要素の周波数特性の変化の様子を図７（Ａ）、（Ｂ）に示す。図７（Ａ）は振幅特性を示すグラフであり、図７（Ｂ）は位相特性を示すグラフである。図７（Ａ）を参照すると、極大値ダンピング係数ｄ_ｆｐが大きくなると振幅極大周波数ｆ_ｆｐは変わらずに振幅極大値のピークがなだらかになる。すなわち、式８で与えられる極大値ダンピング係数ｄ_ｆｐは振幅極大値の鋭さのみに影響することがわかる。
【００５５】
以上のように、本実施の形態においては、振幅極小周波数ｆ_ｆｚおよび極小値ダンピング係数ｄ_ｆｚから式１１を適用して共振相殺フィルタ要素１９の零点ｚ_ｆｒ±ｚ_ｆｉｊを求めることで共振相殺フィルタ要素１９の振幅極小周波数および振幅極小値の鋭さを独立に指定できる。また振幅極大周波数ｆ_ｆｐおよび極大値ダンピング係数ｄ_ｆｐから式１２を適用して共振相殺フィルタ要素１９で極ｐ_ｆｒ±ｐ_ｆｉｊを求めることで共振相殺フィルタ要素１９の振幅極大周波数および振幅極大値の鋭さを独立に指定できる。これにより、駆動系共振特性を相殺するような周波数特性を持つ共振相殺フィルタ要素１９の零点および極を簡便に求めることができる。
【００５６】
次に、共振相殺フィルタ要素１９において用いるパラメータａ、ｂ、ｃ、ｄの算出の仕方について述べる。
【００５７】
式１１および式１２により共振相殺フィルタ要素１９の極の座標（ｐ_ｆｒ±ｐ_ｆｉｊ）および零点の座標（ｚ_ｆｒ±ｚ_ｆｉｊ）は与えられているものとする。ここでは式６に示した演算を行う共振相殺フィルタ要素１９を離散演算を行う制御演算部１０Ａにおいて実現するので、離散演算で共振相殺フィルタ要素１９を実現する。
【００５８】
まず式６に示した共振相殺フィルタ要素１９の極：ｐ_ｆｒ±ｐ_ｆｉｊに相当する、ｚ平面での極Ｐ _ｆｒ±Ｐ _ｆｉｊを式１３を適用して算出する。
【数３９】

……（１３）
【００５９】
また、式６に示した共振相殺フィルタ要素１９の零点の座標（ｚ_ｆｒ±ｚ_ｆｉｊ）に相当する、ｚ平面での零点の座標（Ｚ_ｆｒ±Ｚ_ｆｉｊ）を式１４を適用して算出する。
【数４０】

……（１４）
【００６０】
式１３および式１４を適用して算出されたｚ平面での極、零点をもつ離散型共振相殺フィルタ要素１９の特性は式１５で表される。
【数４１】

……（１５）
【００６１】
式１５から図２に図解した制御演算部１０Ａのパラメータａ、ｂ、ｃ、ｄは、式１６から式１９で規定される。
【数４２】

……（１６）
【数４３】

……（１７）
【数４４】

……（１８）
【数４５】

……（１９）
【００６２】
なお離散時間型共振相殺フィルタ要素１９は式２０で表される数列式で実現される。
【数４６】

……（２０）
ただし、ｉｎ_ｋは時刻ｋでのフィルタへの入力値であり、
ｏｕｔ_ｋは時刻ｋでのフィルタの出力値である。
【００６３】
ここで、１／Ｋ’_ｆ または１／Ｋ _z は式１５における離散時間型共振相殺フィルタ要素１９の直流振幅を１とするような係数であり式２１で規定される。
【数４７】

……（２１）
【００６４】
共振相殺フィルタ用パラメータ設定部４０は上述した演算を行って制御演算部１０Ａにおけるパラメータａ，ｂ，ｃ，ｄを算出して共振相殺フィルタ要素１９に設定する。
【００６５】
図８は共振相殺フィルタ用パラメータ設定部４０の演算動作を示すフローチャートである。以下、共振相殺フィルタ用パラメータ設定部４０の動作を図８を参照して述べる。
【００６６】
ステップ１（Ｓ１）：共振相殺フィルタ用パラメータ設定部４０に、共振相殺フィルタ要素１９の振幅極大周波数ｆ_ｆｐ、極大値ダンピング係数ｄ_ｆｐ、振幅極小周波数ｆ_ｆｚ、極小値ダンピング係数ｄ_ｆｚｚを入力する。
【００６７】
ステップ２（Ｓ２）：共振相殺フィルタ用パラメータ設定部４０は、式１１および式１２の演算を行い、共振相殺フィルタ要素１９の極ｐ_ｆｒ±ｐ_ｆｉｊおよび零点ｚ_ｆｒ±ｚ_ｆｉｊを算出する。
【００６８】
ステップ３（Ｓ３）：共振相殺フィルタ用パラメータ設定部４０は式１３および式１４の演算を行う。これにより、共振相殺フィルタ要素１９の零点の座標（Ｚ_ｆｒ±Ｚ_ｆｉｊ）および極の座標（Ｐ_ｆｒ±Ｐ_ｆｉｊ）が求められる。
【００６９】
ステップ４（Ｓ４）：共振相殺フィルタ用パラメータ設定部４０は式１６から式１９の演算を行いパラメータａ、ｂ、ｃ、ｄを算出し、算出したパラメータａ、ｂ、ｃ、ｄを共振相殺フィルタ要素１９に設定する。
【００７０】
以上により、共振相殺フィルタ要素１９のパラメータａ，ｂ，ｃ，ｄが算出でき、共振相殺フィルタ要素１９に設定されて使用される。
【００７１】
次に共振相殺フィルタ要素１９のパラメータの調整方法について述べる。
図２に図解した共振相殺フィルタ要素１９を用いた制御において駆動系共振特性による振幅最大値の影響で制御系のゲイン余裕が減少するのを防ぐためには共振相殺フィルタ要素１９の零点が駆動系共振極を十分に相殺する必要がある。また共振相殺フィルタ要素１９の極は制御系によって新たに付加される極となるので、制御系の安定性を損なわないように共振相殺フィルタ要素１９のパラメータを設定する必要がある。したがって、共振相殺フィルタ要素１９のパラメータの調整の手順としては、（１）まず駆動系共振極を相殺するよう共振相殺フィルタ要素１９の零点を定め、（２）次に制御系の安定性を損なわないよう共振相殺フィルタの極を決定する。なお、制御系を安定に制御できることが第１なので、共振相殺フィルタ要素１９の極は必ずしも駆動系零点を相殺する必要はない。換言すれば、駆動系の零点を相殺しなくても安定性を損なわないことが第１である。従って制御系の安定性を確認しながら共振相殺フィルタ要素１９の極大値ダンピング係数ｄ_ｆｐおよび振幅極大周波数ｆ_ｆｐを変更する。
【００７２】
図９は共振相殺フィルタ要素１９のパラメータを調整する動作を示すフローチャートである。
【００７３】
ステップ１１：まず、駆動系の振幅周波数特性を測定し振幅極大周波数ｆ_ｐ、振幅極小周波数ｆ_ｚを読み取る。また、極大値ダンピング係数ｄ_ｐ、極小値ダンピング係数ｄ _ｚ を適切な値に定める。
【００７４】
ステップ１２：次に、初期値設定として、共振相殺フィルタ要素１９の振幅極小周波数ｆ_ｆｚ＝ｆ_ｐ、振幅極大周波数ｆ_ｆｐ＝ｆ_ｚとし、極小値ダンピング係数ｄ_ｆｚ＝ｄ_ｐ、極大値ダンピング係数ｄ _ｆｐ ＝ｄ_ｚとする。
【００７５】
ステップ１３：上記初期設定値を共振相殺フィルタ要素１９の初期値とし、共振相殺フィルタ用パラメータ設定部４０に入力する。
【００７６】
ステップ１４、１５：共振相殺フィルタ要素１９を含めた駆動系周波数特性を測定し、駆動系の振幅極大値が十分相殺されているか確認する。
【００７７】
ステップ１６：駆動系の振幅極大値が十分相殺されていなければ、共振相殺フィルタの振幅極小周波数ｆ_ｆｚもしくは極小値ダンピング係数ｄ_ｆｚのいずれかを変更して、ステップ１３の処理に戻り、再度パラメータ設定部に入力する。
【００７８】
振幅極大値が最も小さくなる振幅極小周波数ｆ_ｆｚおよび極小値ダンピング係数ｄ_ｆｚが得られるまで、ステップ１３〜ステップ１５、１６の処理を繰り返す。
【００７９】
ステップ１７：駆動系の振幅極大値が十分相殺されていればそのときの共振相殺フィルタ要素１９の振幅極小周波数ｆ_ｆｚおよび極小値ダンピング係数ｄ_ｆｚを記憶する。
【００８０】
ステップ１８：以上のごとく決定されたパラメータを用いて共振相殺フィルタ要素１９を構成し、共振相殺フィルタ要素１９を含めた図２に図解のサーボ制御装置を動作させる。
【００８１】
ステップ１９：サーボ制御装置の動作結果が安定性に問題がないかを確かめる。
【００８２】
ステップ２０：安定性に問題があるようであれば、共振相殺フィルタ用パラメータ設定部４０はまず極大値ダンピング係数ｄ _ｆｐ を大きくして共振相殺フィルタ用パラメータ設定部４０に入力する。極大値ダンピング係数ｄ_ｆｐを大きくして安定性に問題がなくなれば終了とする。
【００８３】
ステップ２１〜２４：極大値ダンピング係数ｄ_ｆｐを１まで大きくしても安定性に問題があるようであれば、共振相殺フィルタ用パラメータ設定部４０は極大値ダンピング係数ｄ_ｆｐ＝１として、安定性に問題がなくなるまで振幅極大周波数ｆ_ｆｐを大きくして共振相殺フィルタ用パラメータ設定部４０に入力する。
【００８４】
ステップ２５：以上のようにして共振相殺フィルタの特性を決定する。
【００８５】
以上、本発明の実施の形態として、制御対象としてモータを例示し、共振相殺フィルタを有するサーボ制御装置を述べたが、本発明は上述した実施の形態に限定されず種々の制御装置に適用できる。たとえば、制御対象は上述してモータに限らず、ばね特性を示す種々の制御対象およびそのアクチュエータについて適用できる。ばね特性については代表的に２慣性系を例示したが、本発明は他のばね特性を示す制御対象およびアクチュエータについても適用できる。その場合、共振相殺フィルタ要素１９の特性は上述したものとは異なる。
【００８６】
上記実施の形態においては、共振相殺フィルタ要素１９は式６および式１５で表されるフィルタ特性を示す例を述べたが、共振相殺フィルタ要素１９は、２次遅れ要素のみ、または２次進み要素のみの場合についても適用できる。
【００８７】
図２に図解した制御器１００Ａにおいて、ＦＦ制御演算部１２は必須ではない。したがって、本発明はＦＦ制御演算部１２が存在していない制御装置にも適用できる。式１および式２を参照して述べたＦＦ制御演算部１２の演算式およびＦＢ制御演算部１６の演算式は例示であり、制御対象に応じてアルゴリズムで表される。
【００８８】
【発明の効果】
本発明の制御装置においては、制御対象および／またはアクチュエータ（駆動部）のばね特性を相殺する共振相殺フィルタ要素を適用することにより、駆動部のばね要素による共振特性の共振極の影響を相殺させて制御系の安定性を増すことができ、その結果として、良好な動作制御を実現できる。
【００８９】
また本発明によれば、共振周波数およびマージン係数からパラメータの値を算出することで共振相殺フィルタ要素のパラメータを簡便に得ることができ、共振相殺フィルタ要素のパラメータ調整時間を短縮できる。
【図面の簡単な説明】
【図１】 図１は従来のモータを制御対象としたサーボ制御装置の構成図である。
【図２】 図２は本発明に係る制御装置の第１の実施の形態としての、モータを制御対象としたサーボ制御装置の構成図である。
【図３】 図３はばね要素を有する駆動系を２慣性系として表した図である。
【図４】 図４（Ａ）、（Ｂ）は図３に図解した２慣性系の周波特性を示すグラフであり、図４（Ａ）は振幅特性を示すグラフであり、図４（Ｂ）は位相特性を示すグラフである。
【図５】 図５（Ａ）、（Ｂ）は図２に図解した共振相殺フィルタ要素の周波数特性を示すグラフであり、図５（Ａ）は振幅特性を示すグラフであり、図５（Ｂ）は位相特性を示すグラフである。
【図６】 図６（Ａ）、（Ｂ）は本実施の形態において極小値ダンピング係数ｄ_ｆｚを変化させた時の共振相殺フィルタ要素中の２次進み要素の周波数特性の変化の様子を示す図であり、図６（Ａ）は振幅特性を示すグラフであり、図６（Ｂ）は位相特性を示すグラフである。
【図７】 図７（Ａ）、（Ｂ）は本実施の形態において極大値ダンピング係数ｄ_ｆｐを変化させた時の共振相殺フィルタ要素中の２次遅れ要素の周波数特性の変化の様子を示す図であり、図７（Ａ）は振幅特性を示すグラフであり、図７（Ｂ）は位相特性を示すグラフである。
【図８】 図８は図２に図解した共振相殺フィルタ用パラメータ設定部の演算動作を示すフローチャートである。
【図９】 図９は共振相殺フィルタ要素のパラメータを調整する動作を示すフローチャートである。
【符号の説明】
１・・モータ ２・・モータドライバ
１００Ａ・・制御器（コントローラ） ２００・・ロータリィエンコーダ
１０Ａ・・制御演算部
１２・・フィードフォワード（ＦＦ）制御演算部
１４・・加算回路
１６・・フィードバック（ＦＢ）制御演算部
１８・・加算回路
１９・・共振相殺フィルタ要素
２０・・Ｄ／Ａコンバータ、３０・・サンプラ
４０・・共振相殺フィルタ用パラメータ設定部[0001]
BACKGROUND OF THE INVENTION
  The present invention relates to a control method and apparatus for performing servo control of an apparatus having a drive system that requires high-speed positioning control, such as an industrial robot and an electric circuit board component mounting machine.
[0002]
[Prior art]
  Although there are various control targets, for example, even in an industrial robot, the basic control target is an electric motor (motor). Therefore, a motor is typically exemplified as a control target below. There are various types of motors, and for example, servo motors used in industrial robots are exemplified below.
[0003]
  Various methods have been proposed as methods for controlling the servo motor, but the most basic method is to perform control by combining control of proportional (P) operation, integral (I) operation and differential (D) operation. Is the method.
[0004]
  FIG. 1 is a block diagram of a conventional control apparatus according to such a control method. The control device illustrated in FIG. 1 includes a controller (controller) 100 that drives a motor 1 as a control target via a motor drive unit (driver) 2, and a rotary encoder 200 that detects the rotational position of the rotor of the motor 1. Have
[0005]
  The controller (controller) 100 generates a target position signal P_ref(Or target angle signal A_ref) Feedforward control (positive feedback control, FF control) feedforward (FF) control computation unit 12, first adder circuit 14, feedback control (negative feedback control, FB control) computation feedback (FB) The control arithmetic unit 16, the control arithmetic unit 10 having the second adder circuit 18, the digital / analog converter circuit (D / A converter) 20, and the detection signal of the rotary encoder 200 are sampled to the adder circuit 14. It has a sampler 30 to be applied. The controller 100 is a discrete controller that controls by sampling at a predetermined sampling period. The control content is controlled by a feedforward control by the FF control arithmetic unit 12 in the control arithmetic unit 10 and an adder circuit 14. Calculated position deviation = (target position signal P_ref-FB control based on encoder 200 position detection signal)
This is a combination of feedback control by the control unit 16.
[0006]
  The FF control calculation unit 12 performs a control calculation defined by the following formula 1.
[Expression 14]

                                                            ...... (1)
[0007]
  The FB control calculation unit 16 performs a control calculation defined by the following Equation 2.
[Expression 15]

                                                            (2)
[0008]
  In Equations 1 and 2, the control parameter k_pIs a proportional constant (proportional gain or proportional gain), control parameter k_dIs the differential constant (differential gain), control parameter k_iIndicates an integration constant (integration gain). The control parameter α indicates a feed forward gain (gain), and the control parameter β indicates a speed feed forward gain. The symbol T indicates the sampling period, and the variable (z_-1) Indicates a first-order lag element. Therefore, in this example, the FF control calculation unit 12 performs a combination calculation of a proportional (P) control operation and a differential (D) control operation. The FB control calculation unit 16 performs a PID control operation.
[0009]
  The schematic operation of the control apparatus illustrated in FIG. 1 will be described. The FF control calculation unit 12 receives the target position signal P_ref(Or target angle signal A_ref) To perform proportional and derivative (PD) control operations. The adder circuit 14 generates a target position signal P_ref(Or target position signal P_ref) Is subtracted from the actual position detection signal (or angle detection signal) detected by the encoder 200 to calculate the deviation. The FB control calculation unit 16 performs PID calculation on the deviation. The adding circuit 18 adds the calculation result of the FF control calculation unit 12 and the calculation result of the FB control calculation unit 16. The above operations are performed digitally (discretely). The calculation result is converted into an analog signal by the D / A converter 20 and the motor 1 is driven and controlled via the motor driving unit (driver) 2. The result is detected by the encoder 200 and applied to the adder circuit 14 via the sampler 30. This control operation is repeated at a predetermined sampling period.
[0010]
[Problems to be solved by the invention]
  In the servo control device shown in FIG. 1, the control parameter for improving the control performance, for example, the proportional constant k_P, Differential constant k_d, Integral constant k_iHowever, the actual drive system often has resonance characteristics due to the spring characteristics of the drive units such as the motor driver 2 and the motor 1. If the control is performed while ignoring this resonance characteristic, the stability of the control system may be impaired by the resonance characteristic. The spring characteristics will be described later with reference to FIG.
[0011]
  In general, if a control parameter with high control performance (a highly sensitive control parameter used for precise control) is used, the stability of the control system is lowered, and the stability of the control system tends to be impaired by the resonance characteristics. is there. Conversely, in order to maintain the stability of the control system, the drive system must be controlled with a control parameter with low sensitivity (a control parameter with reduced control performance). Then, sufficient control is not performed. As described above, the control has a problem that contradicts itself depending on the sensitivity of the control parameter.
[0012]
  The present invention overcomes the above-mentioned problems relating to control, and the object of the present invention is to provide a stable by applying a resonance canceling filter that cancels even if a “spring element” exists in the mechanism system to be controlled. It is another object of the present invention to provide a control device and a control method that enable precise control.
[0013]
  It is another object of the present invention to provide a resonance cancellation filter parameter adjustment method and an adjustment device that can easily and appropriately adjust the parameters of the resonance cancellation filter.
[0014]
[Means for Solving the Problems]
  In order to achieve the above-described object, the present invention filters a control signal by a resonance cancellation filter including a second-order advance element and / or a second-order delay element in order to cancel resonance characteristics caused by a spring element included in a drive system. To do.
[0015]
  The control device of the present invention has a basic configuration as follows:In a control device for controlling a control target and / or a control system having a spring characteristic in an actuator of the control target by a discrete time method, a deviation calculating unit for obtaining a difference between a target command signal and a position or angle detection signal of the control target; A feedback control calculation unit that performs a feedback control calculation according to the deviation calculated by the deviation calculation unit, and a resonance cancellation filter element that performs a calculation for canceling the spring characteristic with respect to the result.
[0016]
  The resonant cancellation filter element is a secondary advance element that yields an amplitude minimum: (s−z _fr + J × z _fi ) (S-z _fr −j × z _fi ) Or a second order lag element that produces an amplitude maximum: 1 / (s−p _fr + J × p _fi ) (S-p _fr −j × p _fi ) Or both.
  That is, the resonance cancellation filter element is (s−z _fr + Jz _fi ) (S-z _fr -Jz _fi ), And / or [1 / (s−p _fr + Jp _fi ) (S-p _fr −jp _fi )] With a second-order lag element that produces an amplitude maximum.
  For symbols and parameters that define the second-order advance element and the second-order lag element, s is a Laplace operator, j is an imaginary operator, and (z _fr + Jz _fi ), (Z _fr -Jz _fi ) Is the coordinate (z) of the resonance zero of the control system. _fr ± jz _fi ) And z _fr Is the real part of the resonance zero, z _fi Is the imaginary part of the resonance zero and (p _fr + Jp _fi ), (P _fr −jp _fi ) Is the coordinate (p) of the resonance pole of the control system. _fr ± jp _fi ) And p _fr Is the real part of the resonant pole, p _fi Is the imaginary part of the resonant pole.
[0017]
  Preferably, the control device adds a calculation result of the feedforward control calculation unit and a calculation result of the feedback control calculation unit, a feedforward control calculation unit that performs a feedforward control calculation in response to a target command signal And an addition unit,
  The resonance cancellation filter element performs the filtering process on the second-order advance element that brings the amplitude minimum value and / or the second-order lag element that brings the amplitude maximum value on the addition result.
[0018]
  Further preferably, the filter characteristic indicating the secondary advance element that causes the amplitude minimum value of the resonance canceling filter element is expressed by the coordinate of the zero point of the resonance canceling filter element: (z _fr ± jz _fi ) And coordinates of the control object and / or the resonance pole of the actuator: (p _fr ± jp _fi ) And (z _fr ± jz _fi ) = (P _fr ± jp _fi ) Obtained by performing a control analysis that matches
  The second-order lag element filter characteristic that causes the amplitude maximum value of the resonance cancellation filter element is expressed by the coordinate of the pole of the resonance cancellation filter element: (p _fr ± jp _fi ) And the coordinate of the resonance zero of the controlled object and / or actuator: (z _fr ± jz _fi ) And (p _fr ± jp _fi ) = (Z _fr ± jz _fi ) And control analysis matched with
  (Z _fr ± jz _fi ) Is the coordinate of the zero point of the resonance canceling filter element, and (p _fr ± jp _fi ) Are the polar coordinates of the resonance canceling filter element.
[0019]
  Further, the control device of the present invention provides the above-mentionedIn control analysisOf the obtained resonance canceling filter elementThe following parameters a, b, c, d are the resonance canceling filter elementsA resonance canceling filter parameter setting unit set to
  The parameter setting unit for the resonance cancellation filter cancels the spring characteristic, gain kf, parameters a, b, c, d, first order lag element Z ^-1 (Z is the Z conversion operator), secondary delay element Z ^-2 The parameters a, b, c, and d of the resonance cancellation filter element defined by the following equation are obtained and set in the resonance cancellation filter element.
[0020]
[Equation 3]

[0021]
  Preferably, the parameter setting unit for the resonance cancellation filter includes an amplitude minimum frequency f5fz and a minimum value damping coefficient d._fzTo the zero point z of the resonance canceling filter element by applying the following equation:_fr± z_fij is obtained, and the amplitude minimum frequency and the amplitude minimum value of the resonance cancellation filter element are independently designated.
[0022]
[Expression 16]

[0023]
  Preferably, the resonance canceling filter parameter setting unit has an amplitude maximum frequency f._fpAnd the maximum damping coefficient d_fpTo the pole p of the resonant cancellation filter element by applying_fr± p_fij is obtained, and the amplitude maximum frequency and the sharpness of the amplitude maximum value of the resonance cancellation filter element are designated independently.
[Expression 17]

[0024]
  Further, according to the present invention, when the control target and / or the control system having the spring characteristic for the control target actuator is controlled by the discrete time method, the resonance canceling filter process defined by the following equation is performed to cancel the spring characteristic. A parameter setting method for the resonance cancellation filter element in a control method,
[Expression 18]

  The amplitude maximum frequency f of the resonance canceling filter element_fp, Maximum damping coefficient d_fp, Amplitude minimum frequency f_fz, Local minimum damping coefficient d_fzAnd calculate the following formula to determine the poles of the resonance canceling filter element.Coordinates(P_fr± p_fij) and zeroCoordinates(Z_fr± z_fij)
[Equation 19]

[Expression 20]

  The zero point of the resonance canceling filter element is calculated by the following formulaCoordinates(Z_fr± Z_fij) andPolar coordinates(P_fr± P_fij)
[Expression 21]

[Expression 22]

  Calculate the parameters a, b, c, d by calculating the following formula,
[Expression 23]

[Expression 24]

[Expression 25]

[Equation 26]

  The calculated parameters a, b, c, d are set in the resonance canceling filter element.
  A parameter setting method for a resonant cancellation filter is provided.
[0029]
DETAILED DESCRIPTION OF THE INVENTION
  A control apparatus as an embodiment of the control apparatus of the present invention will be described with reference to the accompanying drawings.
[0030]
  FIG. 2 is a configuration diagram of a servo control device that controls a servo motor as an embodiment of the control device according to the present invention.
[0031]
  The configuration of the servo control device illustrated in FIG. 2 is similar to the servo control device illustrated in FIG. 1, but the resonance canceling filter element 19 is included in the control arithmetic unit 10A of the controller (controller) 100A illustrated in FIG. In addition, a resonance cancellation filter parameter setting unit 40 for setting parameters of the resonance cancellation filter element 19 is added to the controller 100A.
[0032]
  That is, the servo control device illustrated in FIG. 2 includes a controller 100A that drives a motor 1 as a control target via a motor drive unit (driver) 2 and a rotary encoder 200 that detects the rotational position of the rotor of the motor 1. Have
[0033]
  The controller 100A receives the target position signal P_ref(Or target angle signal A_ref) Feedforward control (positive feedback control, FF control) feedforward (FF) control computation unit 12, first adder circuit 14, feedback control (negative feedback control, FB control) computation feedback (FB) Control arithmetic unit 10A having control arithmetic unit 16, second addition circuit 18 and resonance cancellation filter element 19, digital / analog conversion circuit (D / A converter) 20, and addition circuit for sampling detection signal of encoder 200 14 and a parameter setting unit 40 for resonance cancellation filter. The controller 100A is also a discrete controller that controls the sampler 30 by sampling at a predetermined sampling period. The control content includes feedforward control in the FF control calculation unit 12 and position deviation calculated in the adder circuit 14 = (Target position signal P_ref(Or target angle signal A_refThis is a combination of feedback control in the FB control calculation unit 16 based on the position detection signal of the encoder 200).
[0034]
  The FF control calculation unit 12 performs a feedforward control calculation according to the control algorithm shown in Equation 1. The FB control calculation unit 16 performs feedback control calculation according to the control algorithm shown in Equation 2. In this example, the FF control calculation unit 12 performs a combination calculation of a proportional (P) control operation and a differential (D) control operation, and the FB control calculation unit 16 performs a PID control operation.
[0035]
  The resonance canceling filter element 19 performs the calculation shown in the following Equation 3.
[Expression 29]

                                                            ...... (3)
[0036]
  The resonance canceling filter element 19 usually includes at least a second-order lag element, but may include a second-order advance element depending on the control target. In the present embodiment, as a representative example, a case including both a secondary delay element and a secondary advance element will be described.
[0037]
  The operation of the servo controller illustrated in FIG. 2 will be described.
  The FF control calculation unit 12 receives the target position signal P_ref(Or target angle signal A_ref) Is subjected to proportional and differential (PD) control calculations based on Equation 1. The adder circuit 14 generates a target position signal P_ref(Or target angle signal A_ref) Is subtracted from the actual position detection signal (or angular position detection signal) detected by the encoder 200 to calculate the deviation. The FB control calculation unit 16 performs PID calculation on the deviation based on Equation 2. The adding circuit 18 adds the calculation result of the FF control calculation unit 12 and the calculation result of the FB control calculation unit 16. The resonance cancellation filter element 19 performs an operation based on Equation 3. The above operations are performed digitally. The calculation result is converted into an analog signal by the D / A converter 20 and the motor 1 is driven and controlled via the motor driving unit 2. The motor driver 2 controls the rotational operation of the motor by adjusting the power supplied to the motor 1 in accordance with the drive signal from the D / A converter 20. The rotation operation result of the motor 1 is detected by the encoder 200 and applied to the adding circuit 14 via the sampler 30 at a predetermined sampling period. This control operation is repeated at a predetermined sampling period.
[0038]
  Hereinafter, the resonance cancellation filter element 19 and the resonance cancellation filter parameter setting unit 40 will be described.
[0039]
  Resonance canceling filter element 19
  The resonance canceling filter element 19 includes a second-order lag element, a second-order advance element, and a gain k as shown in Expression 3._fThus, the calculation defined by Equation 3 is performed to cancel the resonance pole of the drive system and improve the stability of the control system.
  The resonance pole of the drive system is described in detail in Japanese Patent Application Laid-Open No. 8-171402.
[0040]
  The reason why the calculation by the resonance cancellation filter element 19 improves the stability of the control system will be described.
  When a drive system having a spring element (motor 1 and motor driver 2) is generalized, it is represented as a two-inertia system having inertia 1 and inertia 2 as illustrated in FIG. Two inertias 1 and 2 are connected via a spring element. Inertia 1 and 2 are accompanied by viscosities 1 and 2, respectively. The transfer function G (s) of the inertial system illustrated in FIG. 3 can be generally expressed by the following equation.
[30]

                                                            (4)
    However, J₁Is the inertia constant of inertia 1 (inertia constant),
            J₂Is the inertia constant of inertia 2 (inertia constant),
            K is the spring constant of the spring element,
            D₁Is the coefficient of viscous friction of inertia 1
            D₂Is the coefficient of viscous friction of inertia 2
            s is a Laplace operator.
[0041]
  The frequency characteristics of the two inertia system are shown in FIGS. 4 (A) and 4 (B). FIG. 4A is a graph showing amplitude characteristics, and FIG. 4B is a graph showing phase characteristics. 4A and 4B are diagrams showing characteristic curves A1 and P1 when there is a spring element, and characteristic curves A0 and P0 when there is no spring element, respectively. The case where there is no spring element is a case where it is assumed that an ideal rigid body is connected instead of the spring element. Considering the amplitude characteristic illustrated in FIG. 4A, when there is a spring element, the amplitude maximum value due to the resonance pole of the resonance characteristic and the amplitude minimum value due to the resonance zero point are present in the resonance characteristic compared to the case without the spring element. It can be seen. Due to the influence of the amplitude maximum value, the amplitude in the high-circumference region is increased over a wide frequency range, and the amplitude characteristic is increased. For this reason, when considering the open-loop transfer function in the entire control system composed of the control object and the control device composed of the drive unit, the gain margin (gain margin) decreases, the control system becomes unstable, and oscillation easily occurs. .
[0042]
  The resonance canceling filter element 19 illustrated in FIG. 2 is used to overcome this problem and perform highly stable operation control on the drive unit. Hereinafter, the resonance cancellation filter element 19 will be described in detail.
[0043]
  Equation 4 showing the transfer function G (s) of the drive unit can be rewritten as Equation 5. In the following formula:s is a Laplace operator, j is an operator representing an imaginary number, and (z _r + Jz _i ), (Z _r -Jz _i ) Is the coordinate (z) of the resonance zero of the control system. _r ± jz _i ) And z _r Is the real part of the resonance zero, z _i Is the imaginary part of the resonance zero and (p _r + Jp _i ), (P _r −jp _i ) Is the coordinate (p) of the resonance pole of the control system. _r ± jp _i ) And p _r Is the real part of the resonant pole, p _i Is the imaginary part of the resonant pole.
[31]

                                                            ...... (5)
[0044]
  Formula 5Is considered as follows: 1 inertial system: 1 / s (J ′s + D ′), secondary advance element: (s−z_r+ Z_ij) (s-z_r-Z_ij) Second-order lag element: 1 / (s−p_r+ P_ij) (s-p_r-P_ij) and gain K_fCan be considered as a product of However, gain K_fIs a constant gain that makes the amplitude frequency characteristics of Equation 5 and Equation 4 the same.
[0045]
  The minimum amplitude value in FIG. 4A is a secondary advance element: (s−z_r+ Z_ij) (s-z_r-Z_ij), and the amplitude maximum is a second-order lag element: 1 / (s−p_r+ P5ij) (s−p_r-P_ij). Therefore, in the servo controller illustrated in FIG. 2, a secondary advance element that provides an amplitude minimum: (s−z_r+ Z5ij) (s−z_r-Z_ij) and a second-order lag element that produces an amplitude maximum: 1 / (s−p_r+ P_ij) (s-p_r-P_iIt is desirable to insert a component that cancels j). The offset component is shown in Equation 6.
[Expression 32]

                                                            ...... (6)
[0046]
  The resonance canceling filter element 19 basically performs the calculation of Expression 6. However, the zero point of the resonance canceling filter element 19: z _fr ± z_fij and the resonance pole of the drive system: p_r± p_imatch j, ie z_fr± z_fij = p_r± p_ij and the pole of the resonance canceling filter element 19: p_fr± p_fij and resonance zero of drive system: z_r± z_imatch j, i.e. p_fr± p_fij = z _fr ± z _fi By setting j, the resonance pole and resonance zero (FIG. 4A) of the two-inertia system illustrated in FIG. 3 are canceled by the resonance canceling filter element 19, and a pseudo one-inertia system: 1 / s (J ′ s + D ′). As a result, the maximum amplitude value of the frequency characteristic due to the resonance characteristic of the drive system is canceled out, and the frequency characteristic becomes a one-inertia system (curve P0 in FIG. 4A). As a result, a reduction in gain margin when viewed with an open loop transfer function is also remedied, and highly stable control can be performed. As described above, by providing the resonance canceling filter element 19 having the characteristic shown in Expression 3, the stability of the control system is increased and good control can be realized.
[0047]
  Resonance cancellation filter parameter setting unit 40
  Next, the resonance cancellation filter parameter setting unit 40 will be described.
  The resonance canceling filter parameter setting unit 40 illustrated in FIG. 2 has the maximum amplitude frequency f of the resonance canceling filter element 19._fpAnd the maximum damping coefficient d_fpOr the amplitude minimum frequency f of the resonance cancellation filter_fzAnd the minimum damping coefficient d_fzAnd the parameters a, b, c, and d appearing in Equation 3 in the resonance cancellation filter element 19 are output.
[0048]
  The damping coefficient will be described.
  The frequency characteristics of the resonance cancellation filter element 19 are generally illustrated as shown in FIGS. FIG. 5A is a graph showing the amplitude characteristic of the resonance cancellation filter element 19, and FIG. 5B is a graph showing the phase characteristic. As illustrated in FIG. 5A, it has one amplitude minimum value and one amplitude maximum value. In the resonance canceling filter element 19, the frequency f giving the amplitude minimum value_fzAnd the sharpness of the amplitude minimum value is determined by the frequency f of the drive system resonance pole._pBy matching the sharpness with the sharpness, the influence of the drive system resonance pole can be offset. The amplitude maximum frequency f giving the amplitude maximum value of the resonance canceling filter element 19 is also shown._fpAnd the sharpness of the amplitude maximum value is the frequency f of the drive system resonance zero._zIn addition, the influence of the drive system resonance zero can be canceled by matching the sharpness. However, the zero that gives the minimum amplitude characteristic of the resonance canceling filter element 19: z_fr± z_fiTo find j, the real part z of the zero is_frOr imaginary part z_fiChanges the minimum amplitude frequency f_fzAnd the sharpness of the amplitude minimum value changes at the same time, and it takes time and effort to obtain an appropriate zero point of the resonance canceling filter element 19. Similarly, the pole p giving the maximum value characteristic of the resonance cancellation filter element 19_fr± p_fiTo find j, the real part p of the pole_frOr imaginary part p_fi, The amplitude maximum frequency f_fpIn addition, the sharpness of the amplitude maximum value changes at the same time, and it is troublesome to obtain an appropriate pole of the resonance canceling filter element 19.
[0049]
  Therefore, in the present embodiment, a minimum value damping coefficient d corresponding to the sharpness of the amplitude minimum value of the resonance cancellation filter element 19._fzIs a maximum value damping coefficient d corresponding to the sharpness of the amplitude maximum value of the resonance canceling filter element 1._fpIs defined as shown in Equation 8.
[Expression 33]

                                                            ...... (7)
[Expression 34]

                                                            ...... (8)
[0050]
  Amplitude minimum frequency f_fz(Hz) and amplitude maximum frequency f_fp(Hz) is given by Equation 9 and Equation 10, respectively.
[Expression 35]

                                                            ...... (9)
[Expression 36]

                                                          ...... (10)
[0051]
  Thereby, the zero point of the resonance canceling filter element 19: z_fr± z_fij is the minimum amplitude frequency f described above._fzAnd the above minimum damping coefficient d_fzIs obtained as equation (11).
[Expression 37]

                                                          ...... (11)
[0052]
  Minimum value damping coefficient d_fzIn the case of> 1, the zero point is on the real axis and has no vibration and does not contribute to cancellation of resonance. Therefore, 0 <d_fzMinimum value damping coefficient d in the range of ≦ 1_fzWill be set. Minimum value damping coefficient d_fzFIGS. 6A and 6B show how the frequency characteristics of the secondary advance element in the resonance cancellation filter element 19 change when the frequency is changed.
  FIG. 6A is a graph showing amplitude characteristics, and FIG. 6B is a graph showing phase characteristics.
  Referring to FIG. 6A, the minimum value damping coefficient d_fzIncreases, the minimum amplitude frequency f_fzThe negative peak of the amplitude minimum value becomes gentle without changing. That is, the minimum value damping coefficient d shown in Equation 7_fzIt can be seen that only affects the sharpness of the amplitude minimum. Similarly, the pole p of the resonance canceling filter_fr± p_fij is the maximum amplitude frequency f_fpAnd the maximum damping coefficient d_fpIs obtained as equation (11).
[Formula 38]

                                                          (12)
[0053]
  Maximum damping coefficient d_fpIf> 1, the pole is on the real axis and has no vibration and does not contribute to cancellation of resonance. Therefore, in this embodiment, 0 <d_fpMaximum value damping coefficient d in the range of ≦ 1_fpWill be set.
[0054]
  Maximum damping coefficient d_fpFIGS. 7A and 7B show how the frequency characteristics of the second-order lag element in the resonance canceling filter element 19 change when is changed. FIG. 7A is a graph showing amplitude characteristics, and FIG. 7B is a graph showing phase characteristics. Referring to FIG. 7A, the maximum damping coefficient d_fpBecomes larger, the amplitude maximum frequency f_fpThe peak of the amplitude maximum value becomes gentle without changing. That is, the local maximum damping coefficient d given by Equation 8_fpIt can be seen that only affects the sharpness of the amplitude maximum.
[0055]
  As described above, in the present embodiment, the minimum amplitude frequency f_fzAnd the minimum damping coefficient d_fzTo the zero point z of the resonance canceling filter element 19 by applying Equation 11_fr± z_fiBy obtaining j, the amplitude minimum frequency and the amplitude minimum value of the resonance cancellation filter element 19 can be independently designated. The maximum amplitude frequency f_fpAnd the maximum damping coefficient d_fpTo P at the resonance canceling filter element 19 by applying Equation 12_fr± p_fiBy obtaining j, the amplitude maximum frequency and the amplitude maximum value of the resonance cancellation filter element 19 can be independently designated. As a result, the zero point and pole of the resonance canceling filter element 19 having frequency characteristics that cancel the drive system resonance characteristics can be easily obtained.
[0056]
  Next, how to calculate the parameters a, b, c, d used in the resonance cancellation filter element 19 will be described.
[0057]
  The poles of the resonant cancellation filter element 19 are given by Equations 11 and 12.Coordinates(P_fr± p_fij) and zeroCoordinates(Z_fr± z_fij) shall be given. Here, since the resonance cancellation filter element 19 that performs the calculation shown in Expression 6 is realized in the control calculation unit 10A that performs discrete calculation, the resonance cancellation filter element 19 is realized by discrete calculation.
[0058]
  First, the pole of the resonance cancellation filter element 19 shown in Equation 6: p_fr± p_fithe pole in the z plane corresponding to jP _fr±P _fij is calculated by applying equation (13).
[39]

                                                          (13)
[0059]
  Further, the zero point of the resonance canceling filter element 19 shown in Equation 6Coordinates(Z_fr± z_fij) the zero in the z-planeCoordinates(Z_fr± Z_fij) is calculated by applying Equation 14.
[Formula 40]

                                                          (14)
[0060]
  The characteristic of the discrete resonance canceling filter element 19 having poles and zeros in the z plane calculated by applying Expression 13 and Expression 14 is expressed by Expression 15.
[Expression 41]

                                                          ...... (15)
[0061]
  The parameters a, b, c, and d of the control arithmetic unit 10A illustrated in FIG. 2 from Expression 15 are defined by Expression 16 to Expression 19.
[Expression 42]

                                                          ...... (16)
[Expression 43]

                                                          ...... (17)
(44)

                                                          ...... (18)
[Equation 45]

                                                          ...... (19)
[0062]
  The discrete-time resonance cancellation filter element 19 is realized by a numerical sequence expressed by Expression 20.
[Equation 46]

                                                          ...... (20)
        However, in_kIs the input value to the filter at time k,
                out_kIs the output value of the filter at time k.
[0063]
  Where 1 / K ’_f Or 1 / K _z Is a coefficient that makes the DC amplitude of the discrete-time resonance cancellation filter element 19 in Equation 15 equal to 1, and is defined by Equation 21.
[Equation 47]

                                                          (21)
[0064]
  The resonance cancellation filter parameter setting unit 40 calculates the parameters a, b, c, and d in the control calculation unit 10A by performing the above-described calculation, and sets them in the resonance cancellation filter element 19.
[0065]
  FIG. 8 is a flowchart showing the calculation operation of the resonance cancellation filter parameter setting unit 40. The operation of the resonance cancellation filter parameter setting unit 40 will be described below with reference to FIG.
[0066]
  Step 1 (S1): The resonance canceling filter parameter setting unit 40 sets the amplitude maximum frequency f of the resonance canceling filter element 19 to_fp, Maximum damping coefficient d_fp, Amplitude minimum frequency f_fz, Local minimum damping coefficient d_fzEnter z.
[0067]
  Step 2 (S2): The resonance cancellation filter parameter setting unit 40 performs the calculations of Expressions 11 and 12, and the pole p of the resonance cancellation filter element 19_fr± p_fij and zero z_fr± z_fij is calculated.
[0068]
  Step 3 (S3): The resonance canceling filter parameter setting unit 40 performs calculations of Expressions 13 and 14. Thereby, the zero point of the resonance canceling filter element 19 is obtained.Coordinates(Z_fr± Z_fij) andPolar coordinates(P_fr± P_fij) is required.
[0069]
  Step 4 (S4): The resonance canceling filter parameter setting unit 40 calculates the parameters a, b, c, and d by calculating the equations 16 to 19, and uses the calculated parameters a, b, c, and d as the resonance canceling filter. Set to element 19.
[0070]
  As described above, the parameters a, b, c, and d of the resonance canceling filter element 19 can be calculated and set and used in the resonance canceling filter element 19.
[0071]
  Next, a method for adjusting the parameters of the resonance canceling filter element 19 will be described.
  In order to prevent the gain margin of the control system from decreasing due to the influence of the maximum amplitude due to the drive system resonance characteristics in the control using the resonance cancellation filter element 19 illustrated in FIG. 2, the zero point of the resonance cancellation filter element 19 is the drive system resonance. The poles need to be fully offset. Since the pole of the resonance cancellation filter element 19 is a pole newly added by the control system, it is necessary to set the parameters of the resonance cancellation filter element 19 so as not to impair the stability of the control system. Therefore, the procedure for adjusting the parameters of the resonance canceling filter element 19 includes (1) firstly determining the zero point of the resonance canceling filter element 19 so as to cancel the drive system resonance pole, and (2) next, degrading the stability of the control system. The pole of the resonance cancellation filter is determined so as not to exist. Since the control system can be controlled stably, the first is that the pole of the resonance canceling filter element 19 does not necessarily cancel the drive system zero. In other words, the first is that stability is not impaired even if the zeros of the drive system are not canceled out. Accordingly, the maximum damping coefficient d of the resonance canceling filter element 19 is confirmed while confirming the stability of the control system._fpAnd amplitude maximum frequency f_fpTo change.
[0072]
  FIG. 9 is a flowchart showing the operation of adjusting the parameters of the resonance cancellation filter element 19.
[0073]
  Step 11: First, the amplitude frequency characteristic of the drive system is measured to determine the maximum amplitude frequency f._p, Amplitude minimum frequency f_zRead. The maximum damping coefficient d_p, Local minimum damping coefficientd _z Is set to an appropriate value.
[0074]
  Step 12: Next, as the initial value setting, the minimum amplitude frequency f of the resonance cancellation filter element 19_fz= F_p, Maximum amplitude frequency f_fp= F_zAnd the minimum damping coefficient d_fz= D_p, Maximum damping coefficientd _fp = D_zAnd
[0075]
  Step 13: The initial set value is set as the initial value of the resonance cancellation filter element 19 and is input to the resonance cancellation filter parameter setting unit 40.
[0076]
  Steps 14 and 15: Drive system frequency characteristics including the resonance canceling filter element 19 are measured to confirm whether the amplitude maximum value of the drive system is sufficiently canceled.
[0077]
  Step 16: If the amplitude maximum value of the drive system is not sufficiently canceled, the amplitude minimum frequency f of the resonance cancellation filter_fzOr the minimum damping coefficient d_fzAny one of these is changed, and the process returns to the process of step 13 and is input again to the parameter setting unit.
[0078]
  Minimum amplitude frequency f with the minimum amplitude maximum value_fzAnd the minimum damping coefficient d_fzSteps 13 to 15 and 16 are repeated until is obtained.
[0079]
  Step 17: If the amplitude maximum value of the drive system is sufficiently canceled, the amplitude minimum frequency f of the resonance cancellation filter element 19 at that time_fzAnd the minimum damping coefficient d_fzRemember.
[0080]
  Step 18: The resonance canceling filter element 19 is configured using the parameters determined as described above, and the servo controller illustrated in FIG. 2 including the resonance canceling filter element 19 is operated.
[0081]
  Step 19: Check whether the operation result of the servo control device has a problem in stability.
[0082]
  Step 20: If there is a problem in stability, the resonance cancellation filter parameter setting unit 40 first determines the maximum damping coefficient.d _fp And is input to the resonance cancellation filter parameter setting unit 40. Maximum damping coefficient d_fpIf there is no problem with stability by increasing
[0083]
  Steps 21 to 24: Maximum value damping coefficient d_fpIf there is a problem in stability even when the value is increased to 1, the resonance cancellation filter parameter setting unit 40 determines the maximum value damping coefficient d._fp= 1, amplitude maximum frequency f until stability is eliminated_fpAnd is input to the resonance cancellation filter parameter setting unit 40.
[0084]
  Step 25: The characteristic of the resonance cancellation filter is determined as described above.
[0085]
  As described above, as the embodiment of the present invention, the motor is exemplified as the control target and the servo control device having the resonance canceling filter has been described. However, the present invention is not limited to the above-described embodiment and can be applied to various control devices. . For example, the control object is not limited to the motor described above, and can be applied to various control objects that exhibit spring characteristics and their actuators. Although the two-inertia system is typically exemplified for the spring characteristics, the present invention can also be applied to controlled objects and actuators that exhibit other spring characteristics. In that case, the characteristics of the resonance cancellation filter element 19 are different from those described above.
[0086]
  In the above-described embodiment, the example in which the resonance cancellation filter element 19 shows the filter characteristics represented by Expression 6 and Expression 15 has been described. However, the resonance cancellation filter element 19 includes only the second-order lag element or the second-order advance element. It can be applied to the case of only.
[0087]
  In the controller 100A illustrated in FIG. 2, the FF control calculation unit 12 is not essential. Therefore, the present invention can also be applied to a control device in which the FF control calculation unit 12 does not exist. The arithmetic expression of the FF control arithmetic unit 12 and the arithmetic expression of the FB control arithmetic unit 16 described with reference to Expression 1 and Expression 2 are examples, and are expressed by an algorithm according to the controlled object.
[0088]
【The invention's effect】
  In the control device of the present invention, by applying a resonance canceling filter element that cancels the spring characteristic of the object to be controlled and / or the actuator (driving unit), the influence of the resonance pole of the resonance characteristic by the spring element of the driving unit is canceled. Thus, the stability of the control system can be increased, and as a result, good operation control can be realized.
[0089]
  According to the present invention, the parameter value of the resonance cancellation filter element can be easily obtained by calculating the parameter value from the resonance frequency and the margin coefficient, and the parameter adjustment time of the resonance cancellation filter element can be shortened.
[Brief description of the drawings]
FIG. 1 is a configuration diagram of a servo control device that controls a conventional motor.
FIG. 2 is a configuration diagram of a servo control device that controls a motor as a control device according to a first embodiment of the present invention;
FIG. 3 is a diagram showing a drive system having a spring element as a two-inertia system.
4A and 4B are graphs showing the frequency characteristics of the two-inertia system illustrated in FIG. 3, FIG. 4A is a graph showing the amplitude characteristics, and FIG. Is a graph showing phase characteristics.
5A and 5B are graphs showing the frequency characteristics of the resonance canceling filter element illustrated in FIG. 2, FIG. 5A is a graph showing the amplitude characteristics, and FIG. ) Is a graph showing phase characteristics.
6 (A) and 6 (B) are local minimum damping coefficients d in the present embodiment._fzFIG. 6A is a graph showing an amplitude characteristic, and FIG. 6B is a phase diagram. FIG. 6B is a graph showing how the frequency characteristic of the secondary advance element in the resonance canceling filter element changes. It is a graph which shows a characteristic.
7 (A) and 7 (B) are local maximum damping coefficients d in the present embodiment._fpFIG. 7A is a graph showing the amplitude characteristics, and FIG. 7B is a graph showing the amplitude characteristics. FIG. 7B is a graph showing the amplitude characteristics of the second-order lag element in the resonance canceling filter element. It is a graph which shows a characteristic.
FIG. 8 is a flowchart showing the calculation operation of the parameter setting unit for the resonance cancellation filter illustrated in FIG. 2;
FIG. 9 is a flowchart showing an operation of adjusting a parameter of a resonance cancellation filter element.
[Explanation of symbols]
  1 ・ ・ Motor 2 ・ ・ Motor driver
  100A ・ ・ Controller (Controller) 200 ・ ・ Rotary encoder
    10A ・ ・ Control operation part
              12. · Feed forward (FF) control calculation unit
              14. Adder circuit
              16..Feedback (FB) control calculation unit
              18. Adder circuit
              19. Resonance canceling filter element
    20 ... D / A converter, 30 ... sampler
    40 .. Resonance cancellation filter parameter setting section

Claims (7)

In a control apparatus for controlling a control system and / or a control system having a spring characteristic in an actuator of the control object by a discrete time method,
A deviation calculating unit for obtaining a difference between the target command signal and the position or angle detection signal of the control target;
A feedback control calculation unit that performs a feedback control calculation according to the deviation calculated by the deviation calculation unit, and a resonance cancellation filter element that performs a calculation to cancel the spring characteristic with respect to the result,
The resonance canceling filter element includes a secondary advance element that provides an amplitude minimum value defined by (s−z _fr + jz _fi ) (s−z _fr −jz _fi ), and / or [1 / (s−p _fr + jp _fi ) (s-p _fr -jp _fi )] having a second order lag element that yields an amplitude maximum value defined by
However, for the symbols and parameters that define the secondary advance element and the secondary delay element,
s is a Laplace operator,
j is an operator representing an imaginary number,
(Z _fr + jz _fi ), (z _fr −jz _fi ) are the coordinates of the resonance zero of the control system
(Z _fr ± jz _fi )
z _r is the real part of the resonance zero, z _i is the imaginary part of the resonance zero,
(P _fr + jp _fi ), (p _fr −jp _fi ) are the coordinates of the resonance pole of the control system
(P _fr ± jp _fi )
p _fr is the real part of the resonant pole, p _fi is the imaginary part of the resonant pole,
Control device. A feedforward control calculation unit that performs feedforward control calculation in response to a target command signal;
An addition unit for adding the calculation result of the feedforward control calculation unit and the calculation result of the feedback control calculation unit;
The resonance cancellation filter element performs the filtering process on the second-order advance element that brings the amplitude minimum value and / or the second-order lag element that brings the amplitude maximum value on the addition result.
The control device according to claim 1. The filter characteristic indicating the secondary advance element that causes the amplitude minimum value of the resonance canceling filter element includes the coordinates of the zero point of the resonance canceling filter element: (z _fr ± jz _fi ), and the resonance of the control object and / or the actuator. polar coordinates: a _{(p r ± jp i),} (z fr ± jz fi) = (p r ± jp i) and obtained by performing the control analysis matched,
The second-order lag element filter characteristics that cause the amplitude maximum value of the resonance canceling filter element are the coordinates of the pole of the resonance canceling filter element: (p _fr ± jp _fi ) and the coordinates of the resonance zero of the controlled object and / or actuator : a (z _r ± jz _i), obtained by performing a control analysis was consistent with the _{(p fr ± jp fi) =} (z r ± jz i),
Where z _fr ± jz _fi is the coordinate of the zero point of the resonance canceling filter element,
p _fr ± jp _fi is the coordinate of the pole of the resonant cancellation filter element,
The control device according to claim 1 or 2. A resonance cancellation filter parameter setting unit that sets the following parameters a, b, c, and d of the resonance cancellation filter element obtained by the control analysis in the resonance cancellation filter element;
The parameter setting unit for the resonance cancellation filter cancels the spring characteristic, gain kf, parameters a, b, c, d, first order lag element Z ⁻¹ (Z is a Z conversion operator), second order lag element Z ^−2. In order to set the parameters a, b, c, d of the resonance canceling filter element defined by the following formula represented by

Step 1: Input an amplitude maximum frequency f _fp , a maximum value damping coefficient d _fp , an amplitude minimum frequency f _fz , and a minimum value damping coefficient d _fz of the resonance cancellation filter element,
Step 2, calculate the following coordinates to calculate the pole coordinates (p _fr ± jp _fi ) and zero coordinates (z _fr ± jz _fi ) of the resonance canceling filter element,

Step 3, the following equation is calculated to obtain the zero coordinate (z _fr ± jz _fi ) and the polar coordinate (p _fr ± jp _fi ) of the resonance cancellation filter element,

Step 4, calculate the parameters a, b, c, d by calculating the following formula,

Setting the calculated parameters a, b, c, d in the resonance canceling filter element;
The control device according to claim 1. The resonance cancellation filter parameter setting unit includes:
The following equation is applied to obtain the coordinate (z _fr ± jz _fi ) of the zero point of the resonance canceling filter element from the amplitude minimum frequency f _fz and the minimum value damping coefficient d _fz, and the resonance canceling filter element receives the resonance canceling filter element. Specify the amplitude minimum frequency f _fz of the filter element and the sharpness of the amplitude minimum value independently.

The control device according to claim 4. The resonance cancellation filter parameter setting unit includes:
By applying the following equation, the coordinates (p _fr ± jp _fi ) of the pole of the resonance cancellation filter element are obtained from the amplitude maximum frequency f _fp and the maximum value damping coefficient d _fp , and the resonance cancellation filter element is connected to the resonance cancellation filter element. Independently specify the amplitude maximum frequency f _fp and the sharpness of the amplitude maximum value,
The control device according to claim 4.

Gain kf, parameters a, b, c, d, first order lag element Z ⁻¹ for canceling the spring characteristic when controlling the control target and / or the control system having the spring characteristic for the actuator to be controlled by the discrete time method (Z is a Z conversion operator), a parameter setting method for the resonance canceling filter element in a control method for performing the resonance canceling filter processing defined by the following equation, represented by a second-order lag element Z- ² ,

The amplitude maximum frequency f _fp , the maximum value damping coefficient d _fp , the amplitude minimum frequency f _fz , and the minimum value damping coefficient d _fz of the resonance cancellation filter element are input,
The following equation is calculated to calculate the pole coordinates (p _fr ± jp _fi ) and zero coordinates (z _fr ± jz _fi ) of the resonance cancellation filter element,

The calculation of the following equation is performed to obtain the zero coordinate (z _fr ± jz _fi ) and the polar coordinate (p _fr ± jp _fi ) of the resonance canceling filter element,

Calculate the parameters a, b, c, d by calculating

Setting the calculated parameters a, b, c, d in the resonance canceling filter element;
Resonance cancellation filter parameter setting method.

Images