JP4926559B2

JP4926559B2 - 位置制御装置、位置制御方法、ロボット制御装置およびロボット制御方法

Info

Publication number: JP4926559B2
Application number: JP2006175317A
Authority: JP
Inventors: 潔大石; 敏昌宮崎; 広之中田; 敦実橋本; 康士向井
Original assignee: Panasonic Corp; Nagaoka University of Technology; Institute of National Colleges of Technologies Japan; Matsushita Electric Industrial Co Ltd
Current assignee: Panasonic Corp; Nagaoka University of Technology; Institute of National Colleges of Technologies Japan; Panasonic Holdings Corp
Priority date: 2006-06-26
Filing date: 2006-06-26
Publication date: 2012-05-09
Anticipated expiration: 2026-06-26
Also published as: JP2008004007A

Description

本発明は、主に、減速機を介してモータにより駆動される機器に関する位置制御装置および位置制御方法に関し、特に、ロボットにおける振動抑制を行うためのロボット制御装置およびロボットの制御方法に関するものである。

減速機を用いたロボットでは、減速機のばね成分等に起因する共振が存在する。そして、無対策の状態でロボットをこの共振周波数近傍で動作をさせると、振動が発生し、軌跡精度や停止精度に悪影響を与える。

この対策方法としては、サーボループ内で検出信号の共振周波数成分を消去し、共振による振動が発生しにくい制御系を構成する方法（例えば、特許文献１参照）や、動作指令信号から共振周波数成分を消去し、振動が発生する周波数での動作を避ける方法（例えば、特許文献２参照）が知られている。

図７はロボットで用いられる一般的な位置制御ループを示すブロック図である。図７において、位置制御ブロック２１は、位置指令θｃｏｍ１７とモータ１８の位置を検出するエンコーダ１９から得られる位置フィードバック（以下ＦＢと略す）信号θｆｂ２０とを比較し、速度指令ωｃｏｍ２５を速度制御ブロック２２へ出力する。速度制御ブロック２２は、速度指令ωｃｏｍ２５と位置ＦＢ信号θｆｂ２０を微分要素Ｓ２４で微分して得られる速度ＦＢ信号ωｆｂ２６とを比較し、電流指令ｉｃｏｍ２７をサーボアンプ２３に出力する。サーボアンプ２３は電流指令ｉｃｏｍ２７に従ってモータ１８に電流を流す。

ここで、上述したサーボループ内で検出信号の共振周波数成分を消去し、共振による振動が発生しにくい制御系を構成する方法（例えば、特許文献１参照）について、図面を用いて説明する。この方法の例としては、図８や図９に示すように、図７に示す閉ループ内に共振周波数の帯域消去特性を持つノッチフィルタ３１を挿入することが考えられる。なお、図１０は、ノッチフィルタの振幅と位相の周波数特性の一例である。なお、ノッチフィルタ３１は、特定の周波数に急峻な減衰を与えるフィルタである。

しかし、この方法では、共振周波数が特定できれば効果を発揮することができるが、例えば垂直多関節６軸ロボットのようにロボットの姿勢の変化で刻々と共振周波数が変化するような対象に対しては、共振信号のみを正確に消去することは困難である。

そして、共振周波数とノッチフィルタ３１の帯域消去周波数に不一致が発生すると、共振消去をしない場合、すなわちノッチフィルタ１２を挿入していない場合と比べ、位置制御に必要な信号も消去される可能性がある。なお、位置制御に必要な信号が消去されると、閉ループ特性を劣化させ、位置制御自体が不安定になる可能性があり、副作用を出さないで効果を得ることは難しい。

一方、動作指令信号から共振周波数を消去し、振動が発生する周波数での動作を避ける方法（例えば、特許文献２参照）を示したものが図１１である。図１１では、図７の位置指令θｃｏｍ１７の後にノッチフィルタ３１を挿入し、第２の位置指令θｃｏｍ２（２９）を位置制御ブロック２１へ出力する。

この方法では、ノッチフィルタ３１がサーボループの外側に置かれる。そのため、共振周波数とノッチフィルタ３１の帯域消去周波数がずれても、第２の位置指令θｃｏｍ２（２９）に共振しやすい周波数が残り、振動抑制効果が弱くなるだけであり、位置制御の閉ループ特性は従来のままであり、位置制御自体が不安定になるリスクは避けることができる。
特開２００４−１２９４１６号公報特開２００１−２９３６３８号公報

しかしながら、従来の動作指令信号から共振周波数を消去し、振動が発生する周波数での動作を避ける方法にも課題がある。

ロボットの一例である垂直多関節６軸ロボットにおいては、各軸の負荷イナーシャや減速機のばね定数が異なるので、ロボットの軸毎に共振周波数が異なる。

つまり、図１１のノッチフィルタ３１の帯域消去周波数がロボットの軸毎に異なる。帯域消去周波数が異なる場合のノッチフィルタ３１の振幅と位相の周波数特性を図１２に示す。図１２は、ノッチフィルタ３１の帯域消去中心周波数が８Ｈｚの場合の振幅３８および位相４０の周波数特性と、中心周波数が１６Ｈｚの場合の振幅３９および位相４１の周波数特性を示している。なお、図１２では、ロボットがある姿勢である負荷である場合に、ロボットの第１軸の共振周波数が８Ｈｚ、ロボットの第２軸の共振周波数が１６Ｈｚであったとした場合を例として示している。

ここで問題となるのが、位相周波数特性４０と位相周波数特性４１との間の位相差である。図１２より、周波数が１Ｈｚ（１０の０乗Ｈｚ）の場合に、約７°の位相差４２が発生していることがわかる。これは、図１１の位置指令θｃｏｍ１７が１Ｈｚの正弦波であった場合、第２の位置指令θｃｏｍ２（２９）が軸間で約７°の位相差を持つことになる。

このことは、各軸単独で動作させる場合は大きな問題にはならないが、例えばロボットの先端軌跡を直線動作させようとすると、全軸を同期して動作させる必要が出てくる。この時、第２の位置指令θｃｏｍ２（２９）においてロボットの軸間で位相差４２が発生していると、軌跡に誤差が発生する。

以上のことを、以下に示す数式で説明する。一般に用いられるノッチフィルタ３１の伝達関数をＧ(ｓ)とすると、Ｇ(ｓ)は以下の式で表すことが出来る。

ここで、図１１における位置指令θｃｏｍ１７と第２の位置指令θｃｏｍ２（２９）間の誤差をｅθとすると、以下の式で表すことが出来る

ノッチフィルタ帯域消去角周波数ωｃ（＝２・π・ｆｃ）より十分に低い周波数域、すなわちｓ＜＜ωｃの場合は、（数２）において、ｓ/ωｃ＝０とすることにより、以下の様に近似することが出来る。

つまり、図１１においては、ノッチフィルタ帯域消去周波数ｆｃより十分に低い周波数域では、位置指令を微分した速度成分ｓ・θｃｏｍ(s)に比例した位置指令誤差ｅθが常に発生する。また、ノッチフィルタ帯域消去角周波数ｆｃが異なれば、位置指令誤差ｅθにも差が生じることとなる。

すなわち、図１１における帯域消去周波数が異なる場合のノッチフィルタ３１の振幅および位相の周波数特性を示した図１２を（数３）で説明することが出来る。

以上の説明で示したように、従来の方式では、動作指令信号から共振周波数を消去し、振動が発生する周波数での動作を避けることで振動を抑制することは可能であるが、ロボットを構成する各軸で共振周波数が異なる場合に、帯域消去フィルタの位相差により動作軌跡に誤差が生じるという問題があった。

したがって、本発明の目的は、上記課題を解決するものであり、動作指令信号から共振周波数を消去することでロボットの振動を抑制する方式において、ロボットを構成する各軸の共振周波数が異なる場合でも、動作軌跡の誤差を最小限に抑えることができる位置制御装置、位置制御方法、ロボット制御装置およびロボット制御方法を提供することである。

上記目的を達成するために、本発明の位置制御装置は、モータにより駆動する機器に対する位置指令とモータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力する制御ブロックを備え、電流指令によりモータに電流を流す位置制御装置であって、位置指令を微分する微分器と、微分器の出力を入力とし所定の周波数成分を消去する帯域消去フィルタと、帯域消去フィルタで前記所定の周波数成分が消去された信号を積分する積分器と、帯域消去フィルタで前記所定の周波数成分が消去された信号を増幅する増幅器と、積分器の出力と増幅器の出力とを加算して位置制御ブロックに第２の位置指令を出力する加算器とを備え、前記帯域消去フィルタは、前記帯域消去フィルタの伝達関数をＧ（ｓ）とすると、Ｇ（ｓ）が以下の式で示される帯域消去フィルタであり、Ｇ（ｓ）＝（ｓ ^２＋２・ζ２・ωｃ・ｓ＋ωｃ ^２）／（ｓ ^２＋２・ζ１・ωｃ・ｓ＋ωｃ ^２）（ただし、ωｃ＝２・π・ｆｃｆｃ：帯域消去フィルタの帯域消去周波数 ζ１、ζ２：帯域消去の深さと幅を決定するパラメータ。ζ１＞ζ２。）、前記増幅器は、前記帯域消去フィルタで前記所定の周波数成分が消去された信号を（ζ１−ζ２）／（π・ｆｃ）倍に増幅する増幅器であるものである。

本発明の位置制御方法は、モータにより駆動する機器に対する位置指令とモータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力するステップを含み、電流指令によりモータに電流を流す位置制御方法であって、位置指令を微分するステップと、微分した信号のうち所定の周波数成分を消去するステップと、前記消去するステップで所定の周波数成分が消去された信号を積分するステップと、前記消去するステップで所定の周波数成分が消去された信号を増幅するステップと、積分された信号と増幅された信号とを加算して位置フィードバック信号と比較する第２の位置指令を出力するステップとを含み、前記消去するステップは、前記消去するステップに係る伝達関数をＧ（ｓ）とすると、Ｇ（ｓ）が以下の式で示されるステップであり、Ｇ（ｓ）＝（ｓ ^２＋２・ζ２・ωｃ・ｓ＋ωｃ ^２）／（ｓ ^２＋２・ζ１・ωｃ・ｓ＋ωｃ ^２）（ただし、ωｃ＝２・π・ｆｃｆｃ：帯域消去周波数 ζ１、ζ２：帯域消去の深さと幅を決定するパラメータ。ζ１＞ζ２。）、前記増幅するステップは、前記消去するステップで前記所定の周波数成分が消去された信号を（ζ１−ζ２）／（π・ｆｃ）倍に増幅するステップであるものである。

本発明のロボット制御装置は、複数のアームおよび関節軸を有し関節軸の近傍に設けられた減速機を用いてモータにより駆動するロボットに対する位置指令とモータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力する制御ブロックを備え、電流指令によりモータに電流を流すロボット制御装置であって、位置指令を微分する微分器と、微分器の出力を入力とし関節軸の共振周波数を中心周波数とする成分を消去する帯域消去フィルタと、帯域消去フィルタで前記関節軸の共振周波数を中心周波数とする成分が消去された信号を積分する積分器と、帯域消去フィルタで前記関節軸の共振周波数を中心周波数とする成分が消去された信号を増幅する増幅器と、積分器の出力と増幅器の出力とを加算して位置制御ブロックに第２の位置指令を出力する加算器とを備え、前記帯域消去フィルタは、前記帯域消去フィルタの伝達関数をＧ（ｓ）とすると、Ｇ（ｓ）が以下の式で示される帯域消去フィルタであり、Ｇ（ｓ）＝（ｓ ^２＋２・ζ２・ωｃ・ｓ＋ωｃ ^２）／（ｓ ^２＋２・ζ１・ωｃ・ｓ＋ωｃ ^２）（ただし、ωｃ＝２・π・ｆｃｆｃ：帯域消去フィルタの帯域消去周波数 ζ１、ζ２：帯域消去の深さと幅を決定するパラメータ。ζ１＞ζ２。）、前記増幅器は、前記帯域消去フィルタで前記関節軸の共振周波数を中心周波数とする成分が消去された信号を（ζ１−ζ２）／（π・ｆｃ）倍に増幅する増幅器であるものである。

本発明のロボット制御方法は、複数のアームおよび関節軸を有し関節軸の近傍に設けられた減速機を用いてモータにより駆動するロボットに機器に対する位置指令とモータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力するステップを含み、電流指令によりモータに電流を流すロボット制御方法であって、位置指令を微分するステップと、微分した信号のうち関節軸の共振周波数を中心周波数とする周波数成分を消去するステップと、前記消去するステップで前記関節軸の共振周波数を中心周波数とする周波数成分が消去された信号を積分するステップと、前記消去するステップで前記関節軸の共振周波数を中心周波数とする周波数成分が消去された信号を増幅するステップと、積分された信号と増幅された信号とを加算して位置フィードバック信号と比較する第２の位置指令を出力するステップとを含み、前記消去するステップは、前記消去するステップに係る伝達関数をＧ（ｓ）とすると、Ｇ（ｓ）が以下の式で示されるステップであり、Ｇ（ｓ）＝（ｓ ^２＋２・ζ２・ωｃ・ｓ＋ωｃ ^２）／（ｓ ^２＋２・ζ１・ωｃ・ｓ＋ωｃ ^２）（ただし、ωｃ＝２・π・ｆｃｆｃ：帯域消去周波数 ζ１、ζ２：帯域消去の深さと幅を決定するパラメータ。ζ１＞ζ２。）、前記増幅するステップは、前記消去するステップで前記関節軸の共振周波数を中心周波数とする周波数成分が消去された信号を（ζ１−ζ２）／（π・ｆｃ）倍に増幅するステップであるものである。

本発明の位置制御装置によれば、位置指令を微分する微分器と、微分器の出力を入力とし所定の周波数成分を消去する帯域消去フィルタと、帯域消去フィルタの出力を積分する積分器と、帯域消去フィルタの出力を増幅する増幅器と、積分器の出力と増幅器の出力とを加算して位置制御ブロックに第２の位置指令を出力する加算器とを備えたので、位置指令の共振周波数成分を消去する帯域消去フィルタとして速度成分フィードフォワード補償機能を付加したノッチフィルタを用いることで、機器の要素毎で帯域消去周波数が異なっても位置指令誤差を少なくすることができる。これにより、帯域消去フィルタを用いて動作指令信号から共振周波数を消去して振動が発生する周波数での動作を避けることで振動を抑制する際、ノッチフィルタの機器の要素間位相差に起因する軌跡動作の誤差を最小限に抑えることができる。

本発明の位置制御方法によれば、位置指令を微分するステップと、微分した信号のうち所定の周波数成分を消去するステップと、所定の周波数成分が消去された信号を積分するステップと、所定の周波数成分が消去された信号を増幅するステップと、積分された信号と増幅された信号とを加算して位置フィードバック信号と比較する第２の位置指令を出力するステップとを含むので、位置指令の共振周波数成分を消去する際、速度成分フィードフォワード補償機能を付加することで、機器の要素毎で帯域消去周波数が異なっても位置指令誤差を少なくすることができ、上記発明と同様の効果が得られる。

本発明のロボット制御装置によれば、位置指令を微分する微分器と、微分器の出力を入力とし関節軸の共振周波数を中心周波数とする成分を消去する帯域消去フィルタと、帯域消去フィルタの出力を積分する積分器と、帯域消去フィルタの出力を増幅する増幅器と、積分器の出力と増幅器の出力とを加算して位置制御ブロックに第２の位置指令を出力する加算器とを備えたので、位置指令の共振周波数成分を消去する帯域消去フィルタとして速度成分フィードフォワード補償機能を付加したノッチフィルタを用いることで、関節軸毎で帯域消去周波数が異なっても位置指令誤差を少なくすることができる。これにより、帯域消去フィルタを用いて動作指令信号から共振周波数を消去して振動が発生する周波数での動作を避けることで振動を抑制する際、ノッチフィルタの関節軸間位相差に起因する軌跡動作の誤差を最小限に抑えることができる。

本発明のロボット制御方法によれば、位置指令を微分するステップと、微分した信号のうち関節軸の共振周波数を中心周波数とする周波数成分を消去するステップと、周波数成分が消去された信号を積分するステップと、周波数成分が消去された信号を増幅するステップと、積分された信号と増幅された信号とを加算して位置フィードバック信号と比較する第２の位置指令を出力するステップとを含むので、位置指令の共振周波数成分を消去する際、速度成分フィードフォワード補償機能を付加することで、関節軸毎で帯域消去周波数が異なっても位置指令誤差を少なくすることができ、上記発明と同様の効果が得られる。

以下、本発明の実施の形態について、図１から図６を用いて説明する。

図１は、本発明の実施の形態における垂直多関節６軸ロボットの概略構成図であり、ロボットメカ１とロボット制御装置２とから構成される。また、図１には明示していないが、ロボットメカ１は、複数のアームおよび関節軸を有しており、各関節軸の近傍に設けられた減速機を用いて駆動されるものである。

図２は、本発明の実施の形態における垂直多関節ロボットの位置制御に関する構成を示すブロック図であり、ロボットメカ１とロボット制御装置２の内部構成の概略を示す。

図２において、ロボット制御装置２の内部に設けられたメイン制御部４は、操作・教示部３で指示され記憶された軌跡に従い、ロボットの６軸の各軸の位置指令を出力する。そして、その位置指令に追従するように、ロボットの各軸の第１のサーボ制御部５から第６のサーボ制御部１０が、ロボットメカ１内の第１のモータ部１１から第６のモータ部１６をそれぞれ制御する。

図３は、本発明の実施の形態におけるモータ位置指令に位相補償帯域消去フィルタを挿入したロボットの位置制御ループを示すブロック図である。

図３に示すように、ロボットの第１軸目についての第１のサーボ制御部５と第１のモータ部１１とで構成されている。第１のモータ部１１は、モータ１８とエンコーダ１９とから構成されている。また、ロボットの第２〜６軸目も同様に、各々独立してサーボ制御部６〜１０とモータ部１２〜１６とから構成されている。ここで、図３において、従来技術で説明した図７、図８、図９、図１１と同様の要素については、同一の符号を付して詳細な説明は省略する。

図３において、従来の構成と異なるのは、背景技術で説明した図１１におけるノッチフィルタ３１に代えて、詳細な構成は後述するが位相補償帯域消去フィルタ２８を備えた点である。

背景技術の図１１の説明において、（数３）で示したように、位置指令を微分した速度成分ｓ・θｃｏｍ(s)に比例した位置指令誤差ｅθが常に発生していることに着目し、この誤差を削減するため、図３における位相補償帯域消去フィルタ２８は、速度成分フィードフォワード補償機能をノッチフィルタに付加している。この位相補償帯域消去フィルタ２８のブロック図を図４に示す。

図４に示すように、位相補償帯域消去フィルタ２８は、位置指令θｃｏｍ１７を入力する微分器ｓ３０と、微分器ｓ３０の出力を入力とし所定の周波数成分を消去するノッチフィルタ３１と、ノッチフィルタ３１の出力を積分する積分器１／ｓ３２と、ノッチフィルタ３１の出力を増幅する増幅器であるフィードフォワード補償ゲインＫｃｏｍｐ３３と、積分器１／ｓ３２の出力とフィードフォワード補償ゲインＫｃｏｍｐ３３の出力を加算して第２の位置指令θｃｏｍ２（２９）を出力する加算器５０とを備えている。この場合、ノッチフィルタ３１は、微分器ｓ３０の出力を入力としロボットの関節軸の共振周波数を中心周波数とする成分を消去する。

上記のように構成した位置補償帯域消去フィルタ２８の動作について説明する。すなわち、位相補償帯域消去フィルタ２８は、ノッチフィルタ３１の前段に微分器ｓ３０を持ち、図２で示したメイン制御部４から各軸に出力された位置指令θｃｏｍ１７を微分して位置指令速度成分ｄθｃｏｍ４５を生成し、ノッチフィルタ３１に入力する。次に、ノッチフィルタ３１の出力を積分器１／ｓ３２で積分したものと、ノッチフィルタ３１の出力をフィードフォワード補償ゲインＫｃｏｍｐ３３で乗じたものを加算器５０で加算し、加算器５０は第２の位置指令θｃｏｍ２（２９）として出力する。

ここで、フィードフォワードゲインＫｃｏｍｐ３３の決定方法の一例を述べる。

図４において、ノッチフィルタ３１の伝達関数Ｇ(ｓ)を背景技術で説明した（数１）と同じとすると、位置指令θｃｏｍ１７と第２の位置指令θｃｏｍ２（２９）間の位置指令誤差ｅθは以下の式で表すことが出来る。

そして、ノッチフィルタ帯域消去角周波数ωｃ（＝２・π・ｆｃ）より十分に低い周波数域、すなわちｓ＜＜ωｃの場合は、（数４）において、ｓ/ωｃ＝０とすることにより、以下の様に近似することが出来る。

ここで、フィードフォワードゲインＫｃｏｍｐ３３を以下の様に設定すれば、（数５）より求める位置指令誤差ｅθをゼロにすることができる。

つまり、ノッチフィルタ帯域消去周波数ｆｃより十分に低い周波数域では、位置指令誤差ｅθ≒０であり、以下の式が成り立つ。

図４で示した位相補償帯域消去フィルタ２８において、帯域消去周波数が異なる場合の振幅および位相の周波数特性を図６に示す。図６は、例として、ロボットがある姿勢である負荷である場合に、ロボットの第１軸の帯域消去中心周波数が８Ｈｚの場合の振幅３４および位相３６の周波数特性、そして、ロボットの第２軸の中心周波数が１６Ｈｚの場合の振幅３５および位相３７の周波数特性を示している。

背景技術で説明した図１２の振幅３８，３９および位相４０，４１の周波数特性と比較して、図６においては、振幅３４，３５の帯域消去周波数は、図１２の振幅３８，３９から変化は無いが、図６において位相特性３６と位相特性３７の周波数が１Ｈｚ（１０の０乗Ｈｚ）における位相差は、背景技術で説明した図１２の位相特性４０と位相特性４１との間の位相差４２よりも遙かに小さくなっていることがわかる。

つまり、ロボットの先端軌跡を例えば直線動作させようと全軸を同期して動作させる場合でも、動作軌跡の誤差は最小限に抑制することができる。

ここで、図４に示す位相補償帯域消去フィルタ２８を構成するブロック図は、ハード的にも構成することができるが、実際にはソフトウェアで実行されることが多いので、その方法を図５に示すフローチャートを用いて説明する。

図５のフローチャートにおいて、位置指令θｃｏｍを入力（ステップＳ１）した後、これを微分処理したものを図５に示すＳ２の変数ａに記憶する（ステップＳ２）

次に中間変数ａを、ノッチフィルタ処理し中間変数ｂを求める（ステップＳ３）

次に、中間変数ｂを積分して中間変数ｃを求める処理（ステップＳ４）と、フィードフォワード補償ゲインＫｃｏｍｐ３３で乗じて中間変数ｄを求める処理（ステップＳ５）を実行する。

最後に、Ｓ４で算出した中間変数ｃとＳ５で算出したｄを加算し、第２の位置指令θｃｏｍ２（２９）として出力する。（ステップＳ６）

以上により、ソフトウェアにおいてもθｃｏｍ２（２９）を求めることが可能である。

なお、本実施の形態においては、位置制御装置および位置制御方法の例としてはロボットに適用した例を示したが、これに限るものではなく、共振周波数の異なる複数の軸に相当する要素を同時にあるいは個別に駆動し、共振抑制のための帯域消去フィルタの位相差が問題となるロボット以外の機器にも適用できることは言うまでもない。

なお、本実施形態では図３に示すように、位置ループ、速度ループの構成で電流指令を作成する方式としたが、位置ループ速度ループの区別無く電流指令を作成する方式にしてもよい。

本発明の位置制御装置および制御方法によれば、帯域消去フィルタを用いて動作指令信号から共振周波数を消去して振動が発生する周波数での動作を避けることで振動を抑制する際、軸間の位相差に起因する動作軌跡の誤差を最小限に抑えることができるので、共振周波数の異なる複数の軸に相当する要素を同時にあるいは個別に駆動し、共振抑制のための帯域消去フィルタの位相差が問題となる機器にも適用でき、産業上有用である。

本発明の実施の形態における垂直多関節ロボットの概略構成を示す図である。本発明の実施の形態における垂直多関節ロボットの位置制御に関する構成を示すブロック図である。本発明の実施の形態におけるモータ位置指令に位相補償帯域消去フィルタを挿入した制御ループを示すブロック図である。本発明の実施の形態における位相補償帯域消去フィルタの構成を示すブロック図である。本発明の実施の形態における位相補償帯域消去フィルタの処理の流れを示すフローチャートである。本発明の実施の形態におけるノッチフィルタ帯域消去中心周波数が異なる場合の振幅および位相の周波数特性を示す図である。共振の振動抑制が無い従来のモータ位置制御ループを示すブロック図である。従来のモータ位置制御ループ内に共振抑制ノッチフィルタを設けた例を示すブロック図である。従来のモータ位置制御ループ内に共振抑制ノッチフィルタを設けた例を示すブロック図である。従来のノッチフィルタの振幅および位相の周波数特性を示す図である。従来のモータ位置指令に共振抑制ノッチフィルタを挿入した例を示すブロック図である。従来のノッチフィルタ帯域消去中心周波数が異なる場合の振幅および位相の周波数特性を示す図である。

符号の説明

１ロボットメカ
２ロボット制御装置
３操作・教示部
４メイン制御部
５〜１０第１のサーボ制御部〜第６のサーボ制御部
１１〜１６第１のモータ部〜第６のモータ部
１７位置指令θｃｏｍ
１８モータ
１９エンコーダ
２０位置フィードバックθｆｂ
２１位置制御ブロック
２２速度制御ブロック
２３サーボアンプ
２４微分要素ｓ
２５速度指令ωｃｏｍ
２６速度フィードバックωｆｂ
２７モータ電流指令Ｉｃｏｍ
２８位相補償帯域消去フィルタ
２９第２の位置指令θｃｏｍ２
３０微分要素ｓ
３１ノッチフィルタ
３２積分要素１／ｓ
３３フィードフォワード補償ゲインＫｃｏｍｐ
３４位相補償帯域消去フィルタの振幅周波数特性（帯域消去中心周波数８Ｈｚ）
３５位相補償帯域消去フィルタの振幅周波数特性（帯域消去中心周波数１６Ｈｚ）
３６位相補償帯域消去フィルタの位相周波数特性（帯域消去中心周波数８Ｈｚ）
３７位相補償帯域消去フィルタの位相周波数特性（帯域消去中心周波数１６Ｈｚ）
３８ノッチフィルタの振幅周波数特性（帯域消去中心周波数８Ｈｚ）
３９ノッチフィルタの振幅周波数特性（帯域消去中心周波数１６Ｈｚ）
４０ノッチフィルタの位相周波数特性（帯域消去中心周波数８Ｈｚ）
４１ノッチフィルタの位相周波数特性（帯域消去中心周波数１６Ｈｚ）
４２ノッチフィルタの位相差
４３ノッチフィルタの振幅周波数特性
４４ノッチフィルタの位相周波数特性
４５位置指令速度成分ｄθｃｏｍ
５０加算器

Claims

モータにより駆動する機器に対する位置指令と前記モータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力する制御ブロックを備え、前記電流指令により前記モータに電流を流す位置制御装置であって、
前記位置指令を微分する微分器と、
前記微分器の出力を入力とし所定の周波数成分を消去する帯域消去フィルタと、
前記帯域消去フィルタで前記所定の周波数成分が消去された信号を積分する積分器と、
前記帯域消去フィルタで前記所定の周波数成分が消去された信号を増幅する増幅器と、
前記積分器の出力と前記増幅器の出力とを加算して前記位置制御ブロックに第２の位置指令を出力する加算器とを備え、
前記帯域消去フィルタは、前記帯域消去フィルタの伝達関数をＧ（ｓ）とすると、Ｇ（ｓ）が以下の式で示される帯域消去フィルタであり、
Ｇ（ｓ）＝（ｓ ^２＋２・ζ２・ωｃ・ｓ＋ωｃ ^２）／（ｓ ^２＋２・ζ１・ωｃ・ｓ＋ωｃ ^２）
（ただし、ωｃ＝２・π・ｆｃｆｃ：帯域消去フィルタの帯域消去周波数 ζ１、ζ２：帯域消去の深さと幅を決定するパラメータ。ζ１＞ζ２。）、
前記増幅器は、前記帯域消去フィルタで前記所定の周波数成分が消去された信号を（ζ１−ζ２）／（π・ｆｃ）倍に増幅する増幅器である位置制御装置。
モータにより駆動する機器に対する位置指令と前記モータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力するステップを含み、前記電流指令により前記モータに電流を流す位置制御方法であって、
前記位置指令を微分するステップと、
前記微分した信号のうち所定の周波数成分を消去するステップと、
前記消去するステップで前記所定の周波数成分が消去された信号を積分するステップと、
前記消去するステップで前記所定の周波数成分が消去された信号を増幅するステップと、
前記積分された信号と前記増幅された信号とを加算して前記位置フィードバック信号と比較する第２の位置指令を出力するステップとを含み、
前記消去するステップは、前記消去するステップに係る伝達関数をＧ（ｓ）とすると、Ｇ（ｓ）が以下の式で示されるステップであり、
Ｇ（ｓ）＝（ｓ ^２＋２・ζ２・ωｃ・ｓ＋ωｃ ^２）／（ｓ ^２＋２・ζ１・ωｃ・ｓ＋ωｃ ^２）
（ただし、ωｃ＝２・π・ｆｃｆｃ：帯域消去周波数 ζ１、ζ２：帯域消去の深さと幅を決定するパラメータ。ζ１＞ζ２。）、
前記増幅するステップは、前記消去するステップで前記所定の周波数成分が消去された信号を（ζ１−ζ２）／（π・ｆｃ）倍に増幅するステップである位置制御方法。
複数のアームおよび関節軸を有し前記関節軸の近傍に設けられた減速機を用いてモータにより駆動するロボットに対する位置指令と前記モータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力する制御ブロックを備え、前記電流指令により前記モータに電流を流すロボット制御装置であって、
前記位置指令を微分する微分器と、
前記微分器の出力を入力とし前記関節軸の共振周波数を中心周波数とする成分を消去する帯域消去フィルタと、
前記帯域消去フィルタで前記関節軸の共振周波数を中心周波数とする成分が消去された信号を積分する積分器と、
前記帯域消去フィルタで前記関節軸の共振周波数を中心周波数とする成分が消去された信号を増幅する増幅器と、
前記積分器の出力と前記増幅器の出力とを加算して前記位置制御ブロックに第２の位置指令を出力する加算器とを備え、
前記帯域消去フィルタは、前記帯域消去フィルタの伝達関数をＧ（ｓ）とすると、Ｇ（ｓ）が以下の式で示される帯域消去フィルタであり、
Ｇ（ｓ）＝（ｓ ^２＋２・ζ２・ωｃ・ｓ＋ωｃ ^２）／（ｓ ^２＋２・ζ１・ωｃ・ｓ＋ωｃ ^２）
（ただし、ωｃ＝２・π・ｆｃｆｃ：帯域消去フィルタの帯域消去周波数 ζ１、ζ２：帯域消去の深さと幅を決定するパラメータ。ζ１＞ζ２。）、
前記増幅器は、前記帯域消去フィルタで前記関節軸の共振周波数を中心周波数とする成分が消去された信号を（ζ１−ζ２）／（π・ｆｃ）倍に増幅する増幅器であるロボット制御装置。
複数のアームおよび関節軸を有し前記関節軸の近傍に設けられた減速機を用いてモータにより駆動するロボットに機器に対する位置指令と前記モータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力するステップを含み、前記電流指令により前記モータに電流を流すロボット制御方法であって、
前記位置指令を微分するステップと、
前記微分した信号のうち前記関節軸の共振周波数を中心周波数とする周波数成分を消去するステップと、
前記消去するステップで前記関節軸の共振周波数を中心周波数とする周波数成分が消去された信号を積分するステップと、
前記消去するステップで前記関節軸の共振周波数を中心周波数とする周波数成分が消去された信号を増幅するステップと、
前記積分された信号と前記増幅された信号とを加算して前記位置フィードバック信号と比較する第２の位置指令を出力するステップとを含み、
前記消去するステップは、前記消去するステップに係る伝達関数をＧ（ｓ）とすると、Ｇ（ｓ）が以下の式で示されるステップであり、
Ｇ（ｓ）＝（ｓ ^２＋２・ζ２・ωｃ・ｓ＋ωｃ ^２）／（ｓ ^２＋２・ζ１・ωｃ・ｓ＋ωｃ ^２）
（ただし、ωｃ＝２・π・ｆｃｆｃ：帯域消去周波数 ζ１、ζ２：帯域消去の深さと幅を決定するパラメータ。ζ１＞ζ２。）、
前記増幅するステップは、前記消去するステップで前記関節軸の共振周波数を中心周波数とする周波数成分が消去された信号を（ζ１−ζ２）／（π・ｆｃ）倍に増幅するステップであるロボット制御方法。