(57)【要約】【課題】位置指令の共振周波数成分を消去する帯域消去フィルタを用いて振動を抑制する際、ロボットを構成する各軸で共振周波数が違う場合に、帯域消去フィルタの位相差により、動作軌跡に誤差が生じることを防止する。【解決手段】位置指令を微分する微分器３０と、微分器の出力を入力とし関節軸の共振周波数を中心周波数とする成分を消去するノッチフィルタ３１と、ノッチフィルタの出力を積分する積分器３２と、ノッチフィルタの出力を増幅する増幅器３３と、積分器の出力と増幅器の出力とを加算して位置制御ブロックに出力する第２の位置指令を生成する加算器５０とを備えた。帯域消去フィルタとして、速度成分フィードフォワード補償機能を付加したノッチフィルタを用いることにより、軸毎で帯域消去周波数が異なっても、ノッチフィルタの軸間位相差に起因する軌跡動作の誤差を最小限に抑えることができる。【選択図】図４
2008004007.tif 000002
【特許請求の範囲】
【請求項１】
モータにより駆動する機器に対する位置指令と前記モータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力する制御ブロックを備え、前記電流指令により前記モータに電流を流す位置制御装置であって、
前記位置指令を微分する微分器と、
前記微分器の出力を入力とし所定の周波数成分を消去する帯域消去フィルタと、
前記帯域消去フィルタの出力を積分する積分器と、
前記帯域消去フィルタの出力を増幅する増幅器と、
前記積分器の出力と前記増幅器の出力とを加算して前記位置制御ブロックに第２の位置指令を出力する加算器とを備えた位置制御装置。
【請求項２】
モータにより駆動する機器に対する位置指令と前記モータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力するステップを含み、前記電流指令により前記モータに電流を流す位置制御方法であって、
前記位置指令を微分するステップと、
前記微分した信号のうち所定の周波数成分を消去するステップと、
前記所定の周波数成分が消去された信号を積分するステップと、
前記所定の周波数成分が消去された信号を増幅するステップと、
前記積分された信号と前記増幅された信号とを加算して前記位置フィードバック信号と比較する第２の位置指令を出力するステップとを含む位置制御方法。
【請求項３】
複数のアームおよび関節軸を有し前記関節軸の近傍に設けられた減速機を用いてモータにより駆動するロボットに対する位置指令と前記モータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力する制御ブロックを備え、前記電流指令により前記モータに電流を流すロボット制御装置であって、
前記位置指令を微分する微分器と、
前記微分器の出力を入力とし前記関節軸の共振周波数を中心周波数とする成分を消去する帯域消去フィルタと、
前記帯域消去フィルタの出力を積分する積分器と、
前記帯域消去フィルタの出力を増幅する増幅器と、
前記積分器の出力と前記増幅器の出力とを加算して前記位置制御ブロックに第２の位置指令を出力する加算器とを備えたロボット制御装置。
【請求項４】
複数のアームおよび関節軸を有し前記関節軸の近傍に設けられた減速機を用いてモータにより駆動するロボットに機器に対する位置指令と前記モータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力するステップを含み、前記電流指令により前記モータに電流を流すロボット制御方法であって、
前記位置指令を微分するステップと、
前記微分した信号のうち前記関節軸の共振周波数を中心周波数とする周波数成分を消去するステップと、
前記周波数成分が消去された信号を積分するステップと、
前記周波数成分が消去された信号を増幅するステップと、
前記積分された信号と前記増幅された信号とを加算して前記位置フィードバック信号と比較する第２の位置指令を出力するステップとを含むロボット制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、主に、減速機を介してモータにより駆動される機器に関する位置制御装置および位置制御方法に関し、特に、ロボットにおける振動抑制を行うためのロボット制御装置およびロボットの制御方法に関するものである。
【背景技術】
【０００２】
減速機を用いたロボットでは、減速機のばね成分等に起因する共振が存在する。そして、無対策の状態でロボットをこの共振周波数近傍で動作をさせると、振動が発生し、軌跡精度や停止精度に悪影響を与える。
【０００３】
この対策方法としては、サーボループ内で検出信号の共振周波数成分を消去し、共振による振動が発生しにくい制御系を構成する方法（例えば、特許文献１参照）や、動作指令信号から共振周波数成分を消去し、振動が発生する周波数での動作を避ける方法（例えば、特許文献２参照）が知られている。
【０００４】
図７はロボットで用いられる一般的な位置制御ループを示すブロック図である。図７において、位置制御ブロック２１は、位置指令θｃｏｍ１７とモータ１８の位置を検出するエンコーダ１９から得られる位置フィードバック（以下ＦＢと略す）信号θｆｂ２０とを比較し、速度指令ωｃｏｍ２５を速度制御ブロック２２へ出力する。速度制御ブロック２２は、速度指令ωｃｏｍ２５と位置ＦＢ信号θｆｂ２０を微分要素Ｓ２４で微分して得られる速度ＦＢ信号ωｆｂ２６とを比較し、電流指令ｉｃｏｍ２７をサーボアンプ２３に出力する。サーボアンプ２３は電流指令ｉｃｏｍ２７に従ってモータ１８に電流を流す。
【０００５】
ここで、上述したサーボループ内で検出信号の共振周波数成分を消去し、共振による振動が発生しにくい制御系を構成する方法（例えば、特許文献１参照）について、図面を用いて説明する。この方法の例としては、図８や図９に示すように、図７に示す閉ループ内に共振周波数の帯域消去特性を持つノッチフィルタ３１を挿入することが考えられる。なお、図１０は、ノッチフィルタの振幅と位相の周波数特性の一例である。なお、ノッチフィルタ３１は、特定の周波数に急峻な減衰を与えるフィルタである。
【０００６】
しかし、この方法では、共振周波数が特定できれば効果を発揮することができるが、例えば垂直多関節６軸ロボットのようにロボットの姿勢の変化で刻々と共振周波数が変化するような対象に対しては、共振信号のみを正確に消去することは困難である。
【０００７】
そして、共振周波数とノッチフィルタ３１の帯域消去周波数に不一致が発生すると、共振消去をしない場合、すなわちノッチフィルタ１２を挿入していない場合と比べ、位置制御に必要な信号も消去される可能性がある。なお、位置制御に必要な信号が消去されると、閉ループ特性を劣化させ、位置制御自体が不安定になる可能性があり、副作用を出さないで効果を得ることは難しい。
【０００８】
一方、動作指令信号から共振周波数を消去し、振動が発生する周波数での動作を避ける方法（例えば、特許文献２参照）を示したものが図１１である。図１１では、図７の位置指令θｃｏｍ１７の後にノッチフィルタ３１を挿入し、第２の位置指令θｃｏｍ２（２９）を位置制御ブロック２１へ出力する。
【０００９】
この方法では、ノッチフィルタ３１がサーボループの外側に置かれる。そのため、共振周波数とノッチフィルタ３１の帯域消去周波数がずれても、第２の位置指令θｃｏｍ２（２９）に共振しやすい周波数が残り、振動抑制効果が弱くなるだけであり、位置制御の閉ループ特性は従来のままであり、位置制御自体が不安定になるリスクは避けることができる。
【特許文献１】特開２００４−１２９４１６号公報
【特許文献２】特開２００１−２９３６３８号公報
【発明の開示】
【発明が解決しようとする課題】
【００１０】
しかしながら、従来の動作指令信号から共振周波数を消去し、振動が発生する周波数での動作を避ける方法にも課題がある。
【００１１】
ロボットの一例である垂直多関節６軸ロボットにおいては、各軸の負荷イナーシャや減速機のばね定数が異なるので、ロボットの軸毎に共振周波数が異なる。
【００１２】
つまり、図１１のノッチフィルタ３１の帯域消去周波数がロボットの軸毎に異なる。帯域消去周波数が異なる場合のノッチフィルタ３１の振幅と位相の周波数特性を図１２に示す。図１２は、ノッチフィルタ３１の帯域消去中心周波数が８Ｈｚの場合の振幅３８および位相４０の周波数特性と、中心周波数が１６Ｈｚの場合の振幅３９および位相４１の周波数特性を示している。なお、図１２では、ロボットがある姿勢である負荷である場合に、ロボットの第１軸の共振周波数が８Ｈｚ、ロボットの第２軸の共振周波数が１６Ｈｚであったとした場合を例として示している。
【００１３】
ここで問題となるのが、位相周波数特性４０と位相周波数特性４１との間の位相差である。図１２より、周波数が１Ｈｚ（１０の０乗Ｈｚ）の場合に、約７°の位相差４２が発生していることがわかる。これは、図１１の位置指令θｃｏｍ１７が１Ｈｚの正弦波であった場合、第２の位置指令θｃｏｍ２（２９）が軸間で約７°の位相差を持つことになる。
【００１４】
このことは、各軸単独で動作させる場合は大きな問題にはならないが、例えばロボットの先端軌跡を直線動作させようとすると、全軸を同期して動作させる必要が出てくる。この時、第２の位置指令θｃｏｍ２（２９）においてロボットの軸間で位相差４２が発生していると、軌跡に誤差が発生する。
【００１５】
以上のことを、以下に示す数式で説明する。一般に用いられるノッチフィルタ３１の伝達関数をＧ(ｓ)とすると、Ｇ(ｓ)は以下の式で表すことが出来る。
【００１６】
【数１】
2008004007.tif 000003
ここで、図１１における位置指令θｃｏｍ１７と第２の位置指令θｃｏｍ２（２９）間の誤差をｅθとすると、以下の式で表すことが出来る
【００１７】
【数２】
2008004007.tif 000004
ノッチフィルタ帯域消去角周波数ωｃ（＝２・π・ｆｃ）より十分に低い周波数域、すなわちｓ＜＜ωｃの場合は、（数２）において、ｓ/ωｃ＝０とすることにより、以下の様に近似することが出来る。
【００１８】
【数３】
2008004007.tif 000005
つまり、図１１においては、ノッチフィルタ帯域消去周波数ｆｃより十分に低い周波数域では、位置指令を微分した速度成分ｓ・θｃｏｍ(s)に比例した位置指令誤差ｅθが常に発生する。また、ノッチフィルタ帯域消去角周波数ｆｃが異なれば、位置指令誤差ｅθにも差が生じることとなる。
【００１９】
すなわち、図１１における帯域消去周波数が異なる場合のノッチフィルタ３１の振幅および位相の周波数特性を示した図１２を（数３）で説明することが出来る。
【００２０】
以上の説明で示したように、従来の方式では、動作指令信号から共振周波数を消去し、振動が発生する周波数での動作を避けることで振動を抑制することは可能であるが、ロボットを構成する各軸で共振周波数が異なる場合に、帯域消去フィルタの位相差により動作軌跡に誤差が生じるという問題があった。
【００２１】
したがって、本発明の目的は、上記課題を解決するものであり、動作指令信号から共振周波数を消去することでロボットの振動を抑制する方式において、ロボットを構成する各軸の共振周波数が異なる場合でも、動作軌跡の誤差を最小限に抑えることができる位置制御装置、位置制御方法、ロボット制御装置およびロボット制御方法を提供することである。
【課題を解決するための手段】
【００２２】
上記目的を達成するために、本発明の請求項１記載の位置制御装置は、モータにより駆動する機器に対する位置指令とモータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力する制御ブロックを備え、電流指令によりモータに電流を流す位置制御装置であって、位置指令を微分する微分器と、微分器の出力を入力とし所定の周波数成分を消去する帯域消去フィルタと、帯域消去フィルタの出力を積分する積分器と、帯域消去フィルタの出力を増幅する増幅器と、積分器の出力と増幅器の出力とを加算して位置制御ブロックに第２の位置指令を出力する加算器とを備えたものである。
【００２３】
請求項２記載の位置制御方法は、モータにより駆動する機器に対する位置指令とモータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力するステップを含み、電流指令によりモータに電流を流す位置制御方法であって、位置指令を微分するステップと、微分した信号のうち所定の周波数成分を消去するステップと、所定の周波数成分が消去された信号を積分するステップと、所定の周波数成分が消去された信号を増幅するステップと、積分された信号と増幅された信号とを加算して位置フィードバック信号と比較する第２の位置指令を出力するステップとを含むものである。
【００２４】
請求項３記載のロボット制御装置は、複数のアームおよび関節軸を有し関節軸の近傍に設けられた減速機を用いてモータにより駆動するロボットに対する位置指令とモータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力する制御ブロックを備え、電流指令によりモータに電流を流すロボット制御装置であって、位置指令を微分する微分器と、微分器の出力を入力とし関節軸の共振周波数を中心周波数とする成分を消去する帯域消去フィルタと、帯域消去フィルタの出力を積分する積分器と、帯域消去フィルタの出力を増幅する増幅器と、積分器の出力と増幅器の出力とを加算して位置制御ブロックに第２の位置指令を出力する加算器とを備えたものである。
【００２５】
請求項４記載のロボット制御方法は、複数のアームおよび関節軸を有し関節軸の近傍に設けられた減速機を用いてモータにより駆動するロボットに機器に対する位置指令とモータの位置を検出して得られる位置フィードバック信号とを比較し、電流指令を出力するステップを含み、電流指令によりモータに電流を流すロボット制御方法であって、位置指令を微分するステップと、微分した信号のうち関節軸の共振周波数を中心周波数とする周波数成分を消去するステップと、周波数成分が消去された信号を積分するステップと、周波数成分が消去された信号を増幅するステップと、積分された信号と増幅された信号とを加算して位置フィードバック信号と比較する第２の位置指令を出力するステップとを含むものである。
【発明の効果】
【００２６】
本発明の位置制御装置によれば、位置指令を微分する微分器と、微分器の出力を入力とし所定の周波数成分を消去する帯域消去フィルタと、帯域消去フィルタの出力を積分する積分器と、帯域消去フィルタの出力を増幅する増幅器と、積分器の出力と増幅器の出力とを加算して位置制御ブロックに第２の位置指令を出力する加算器とを備えたので、位置指令の共振周波数成分を消去する帯域消去フィルタとして速度成分フィードフォワード補償機能を付加したノッチフィルタを用いることで、機器の要素毎で帯域消去周波数が異なっても位置指令誤差を少なくすることができる。これにより、帯域消去フィルタを用いて動作指令信号から共振周波数を消去して振動が発生する周波数での動作を避けることで振動を抑制する際、ノッチフィルタの機器の要素間位相差に起因する軌跡動作の誤差を最小限に抑えることができる。
【００２７】
本発明の位置制御方法によれば、位置指令を微分するステップと、微分した信号のうち所定の周波数成分を消去するステップと、所定の周波数成分が消去された信号を積分するステップと、所定の周波数成分が消去された信号を増幅するステップと、積分された信号と増幅された信号とを加算して位置フィードバック信号と比較する第２の位置指令を出力するステップとを含むので、位置指令の共振周波数成分を消去する際、速度成分フィードフォワード補償機能を付加することで、機器の要素毎で帯域消去周波数が異なっても位置指令誤差を少なくすることができ、上記発明と同様の効果が得られる。
【００２８】
本発明のロボット制御装置によれば、位置指令を微分する微分器と、微分器の出力を入力とし関節軸の共振周波数を中心周波数とする成分を消去する帯域消去フィルタと、帯域消去フィルタの出力を積分する積分器と、帯域消去フィルタの出力を増幅する増幅器と、積分器の出力と増幅器の出力とを加算して位置制御ブロックに第２の位置指令を出力する加算器とを備えたので、位置指令の共振周波数成分を消去する帯域消去フィルタとして速度成分フィードフォワード補償機能を付加したノッチフィルタを用いることで、関節軸毎で帯域消去周波数が異なっても位置指令誤差を少なくすることができる。これにより、帯域消去フィルタを用いて動作指令信号から共振周波数を消去して振動が発生する周波数での動作を避けることで振動を抑制する際、ノッチフィルタの関節軸間位相差に起因する軌跡動作の誤差を最小限に抑えることができる。
【００２９】
本発明のロボット制御方法によれば、位置指令を微分するステップと、微分した信号のうち関節軸の共振周波数を中心周波数とする周波数成分を消去するステップと、周波数成分が消去された信号を積分するステップと、周波数成分が消去された信号を増幅するステップと、積分された信号と増幅された信号とを加算して位置フィードバック信号と比較する第２の位置指令を出力するステップとを含むので、位置指令の共振周波数成分を消去する際、速度成分フィードフォワード補償機能を付加することで、関節軸毎で帯域消去周波数が異なっても位置指令誤差を少なくすることができ、上記発明と同様の効果が得られる。
【発明を実施するための最良の形態】
【００３０】
以下、本発明の実施の形態について、図１から図６を用いて説明する。
【００３１】
図１は、本発明の実施の形態における垂直多関節６軸ロボットの概略構成図であり、ロボットメカ１とロボット制御装置２とから構成される。また、図１には明示していないが、ロボットメカ１は、複数のアームおよび関節軸を有しており、各関節軸の近傍に設けられた減速機を用いて駆動されるものである。
【００３２】
図２は、本発明の実施の形態における垂直多関節ロボットの位置制御に関する構成を示すブロック図であり、ロボットメカ１とロボット制御装置２の内部構成の概略を示す。
【００３３】
図２において、ロボット制御装置２の内部に設けられたメイン制御部４は、操作・教示部３で指示され記憶された軌跡に従い、ロボットの６軸の各軸の位置指令を出力する。そして、その位置指令に追従するように、ロボットの各軸の第１のサーボ制御部５から第６のサーボ制御部１０が、ロボットメカ１内の第１のモータ部１１から第６のモータ部１６をそれぞれ制御する。
【００３４】
図３は、本発明の実施の形態におけるモータ位置指令に位相補償帯域消去フィルタを挿入したロボットの位置制御ループを示すブロック図である。
【００３５】
図３に示すように、ロボットの第１軸目についての第１のサーボ制御部５と第１のモータ部１１とで構成されている。第１のモータ部１１は、モータ１８とエンコーダ１９とから構成されている。また、ロボットの第２〜６軸目も同様に、各々独立してサーボ制御部６〜１０とモータ部１２〜１６とから構成されている。ここで、図３において、従来技術で説明した図７、図８、図９、図１１と同様の要素については、同一の符号を付して詳細な説明は省略する。
【００３６】
図３において、従来の構成と異なるのは、背景技術で説明した図１１におけるノッチフィルタ３１に代えて、詳細な構成は後述するが位相補償帯域消去フィルタ２８を備えた点である。
【００３７】
背景技術の図１１の説明において、（数３）で示したように、位置指令を微分した速度成分ｓ・θｃｏｍ(s)に比例した位置指令誤差ｅθが常に発生していることに着目し、この誤差を削減するため、図３における位相補償帯域消去フィルタ２８は、速度成分フィードフォワード補償機能をノッチフィルタに付加している。この位相補償帯域消去フィルタ２８のブロック図を図４に示す。
【００３８】
図４に示すように、位相補償帯域消去フィルタ２８は、位置指令θｃｏｍ１７を入力する微分器ｓ３０と、微分器ｓ３０の出力を入力とし所定の周波数成分を消去するノッチフィルタ３１と、ノッチフィルタ３１の出力を積分する積分器１／ｓ３２と、ノッチフィルタ３１の出力を増幅する増幅器であるフィードフォワード補償ゲインＫｃｏｍｐ３３と、積分器１／ｓ３２の出力とフィードフォワード補償ゲインＫｃｏｍｐ３３の出力を加算して第２の位置指令θｃｏｍ２（２９）を出力する加算器５０とを備えている。この場合、ノッチフィルタ３１は、微分器ｓ３０の出力を入力としロボットの関節軸の共振周波数を中心周波数とする成分を消去する。
【００３９】
上記のように構成した位置補償帯域消去フィルタ２８の動作について説明する。すなわち、位相補償帯域消去フィルタ２８は、ノッチフィルタ３１の前段に微分器ｓ３０を持ち、図２で示したメイン制御部４から各軸に出力された位置指令θｃｏｍ１７を微分して位置指令速度成分ｄθｃｏｍ４５を生成し、ノッチフィルタ３１に入力する。次に、ノッチフィルタ３１の出力を積分器１／ｓ３２で積分したものと、ノッチフィルタ３１の出力をフィードフォワード補償ゲインＫｃｏｍｐ３３で乗じたものを加算器５０で加算し、加算器５０は第２の位置指令θｃｏｍ２（２９）として出力する。
【００４０】
ここで、フィードフォワードゲインＫｃｏｍｐ３３の決定方法の一例を述べる。
【００４１】
図４において、ノッチフィルタ３１の伝達関数Ｇ(ｓ)を背景技術で説明した（数１）と同じとすると、位置指令θｃｏｍ１７と第２の位置指令θｃｏｍ２（２９）間の位置指令誤差ｅθは以下の式で表すことが出来る。
【００４２】
【数４】
2008004007.tif 000006
そして、ノッチフィルタ帯域消去角周波数ωｃ（＝２・π・ｆｃ）より十分に低い周波数域、すなわちｓ＜＜ωｃの場合は、（数４）において、ｓ/ωｃ＝０とすることにより、以下の様に近似することが出来る。
【００４３】
【数５】
2008004007.tif 000007
ここで、フィードフォワードゲインＫｃｏｍｐ３３を以下の様に設定すれば、（数５）より求める位置指令誤差ｅθをゼロにすることができる。
【００４４】
【数６】
2008004007.tif 000008
つまり、ノッチフィルタ帯域消去周波数ｆｃより十分に低い周波数域では、位置指令誤差ｅθ≒０であり、以下の式が成り立つ。
【００４５】
【数７】
2008004007.tif 000009
図４で示した位相補償帯域消去フィルタ２８において、帯域消去周波数が異なる場合の振幅および位相の周波数特性を図６に示す。図６は、例として、ロボットがある姿勢である負荷である場合に、ロボットの第１軸の帯域消去中心周波数が８Ｈｚの場合の振幅３４および位相３６の周波数特性、そして、ロボットの第２軸の中心周波数が１６Ｈｚの場合の振幅３５および位相３７の周波数特性を示している。
【００４６】
背景技術で説明した図１２の振幅３８，３９および位相４０，４１の周波数特性と比較して、図６においては、振幅３４，３５の帯域消去周波数は、図１２の振幅３８，３９から変化は無いが、図６において位相特性３６と位相特性３７の周波数が１Ｈｚ（１０の０乗Ｈｚ）における位相差は、背景技術で説明した図１２の位相特性４０と位相特性４１との間の位相差４２よりも遙かに小さくなっていることがわかる。
【００４７】
つまり、ロボットの先端軌跡を例えば直線動作させようと全軸を同期して動作させる場合でも、動作軌跡の誤差は最小限に抑制することができる。
【００４８】
ここで、図４に示す位相補償帯域消去フィルタ２８を構成するブロック図は、ハード的にも構成することができるが、実際にはソフトウェアで実行されることが多いので、その方法を図５に示すフローチャートを用いて説明する。
【００４９】
図５のフローチャートにおいて、位置指令θｃｏｍを入力（ステップＳ１）した後、これを微分処理したものを図５に示すＳ２の変数ａに記憶する（ステップＳ２）
【００５０】
【数８】
2008004007.tif 000010
次に中間変数ａを、ノッチフィルタ処理し中間変数ｂを求める（ステップＳ３）
【００５１】
【数９】
2008004007.tif 000011
次に、中間変数ｂを積分して中間変数ｃを求める処理（ステップＳ４）と、フィードフォワード補償ゲインＫｃｏｍｐ３３で乗じて中間変数ｄを求める処理（ステップＳ５）を実行する。
【００５２】
【数１０】
2008004007.tif 000012
最後に、Ｓ４で算出した中間変数ｃとＳ５で算出したｄを加算し、第２の位置指令θｃｏｍ２（２９）として出力する。（ステップＳ６）
【００５３】
【数１１】
2008004007.tif 000013
以上により、ソフトウェアにおいてもθｃｏｍ２（２９）を求めることが可能である。
【００５４】
なお、本実施の形態においては、位置制御装置および位置制御方法の例としてはロボットに適用した例を示したが、これに限るものではなく、共振周波数の異なる複数の軸に相当する要素を同時にあるいは個別に駆動し、共振抑制のための帯域消去フィルタの位相差が問題となるロボット以外の機器にも適用できることは言うまでもない。
【００５５】
なお、本実施形態では図３に示すように、位置ループ、速度ループの構成で電流指令を作成する方式としたが、位置ループ速度ループの区別無く電流指令を作成する方式にしてもよい。
【産業上の利用可能性】
【００５６】
本発明の位置制御装置および制御方法によれば、帯域消去フィルタを用いて動作指令信号から共振周波数を消去して振動が発生する周波数での動作を避けることで振動を抑制する際、軸間の位相差に起因する動作軌跡の誤差を最小限に抑えることができるので、共振周波数の異なる複数の軸に相当する要素を同時にあるいは個別に駆動し、共振抑制のための帯域消去フィルタの位相差が問題となる機器にも適用でき、産業上有用である。
【図面の簡単な説明】
【００５７】
【図１】本発明の実施の形態における垂直多関節ロボットの概略構成を示す図である。
【図２】本発明の実施の形態における垂直多関節ロボットの位置制御に関する構成を示すブロック図である。
【図３】本発明の実施の形態におけるモータ位置指令に位相補償帯域消去フィルタを挿入した制御ループを示すブロック図である。
【図４】本発明の実施の形態における位相補償帯域消去フィルタの構成を示すブロック図である。
【図５】本発明の実施の形態における位相補償帯域消去フィルタの処理の流れを示すフローチャートである。
【図６】本発明の実施の形態におけるノッチフィルタ帯域消去中心周波数が異なる場合の振幅および位相の周波数特性を示す図である。
【図７】共振の振動抑制が無い従来のモータ位置制御ループを示すブロック図である。
【図８】従来のモータ位置制御ループ内に共振抑制ノッチフィルタを設けた例を示すブロック図である。
【図９】従来のモータ位置制御ループ内に共振抑制ノッチフィルタを設けた例を示すブロック図である。
【図１０】従来のノッチフィルタの振幅および位相の周波数特性を示す図である。
【図１１】従来のモータ位置指令に共振抑制ノッチフィルタを挿入した例を示すブロック図である。
【図１２】従来のノッチフィルタ帯域消去中心周波数が異なる場合の振幅および位相の周波数特性を示す図である。
【符号の説明】
【００５８】
１  ロボットメカ
２  ロボット制御装置
３  操作・教示部
４  メイン制御部
５〜１０  第１のサーボ制御部〜第６のサーボ制御部
１１〜１６  第１のモータ部〜第６のモータ部
１７  位置指令θｃｏｍ
１８  モータ
１９  エンコーダ
２０  位置フィードバックθｆｂ
２１  位置制御ブロック
２２  速度制御ブロック
２３  サーボアンプ
２４  微分要素ｓ
２５  速度指令ωｃｏｍ
２６  速度フィードバックωｆｂ
２７  モータ電流指令Ｉｃｏｍ
２８  位相補償帯域消去フィルタ
２９  第２の位置指令θｃｏｍ２
３０  微分要素ｓ
３１  ノッチフィルタ
３２  積分要素１／ｓ
３３  フィードフォワード補償ゲインＫｃｏｍｐ
３４  位相補償帯域消去フィルタの振幅周波数特性（帯域消去中心周波数８Ｈｚ）
３５  位相補償帯域消去フィルタの振幅周波数特性（帯域消去中心周波数１６Ｈｚ）
３６  位相補償帯域消去フィルタの位相周波数特性（帯域消去中心周波数８Ｈｚ）
３７  位相補償帯域消去フィルタの位相周波数特性（帯域消去中心周波数１６Ｈｚ）
３８  ノッチフィルタの振幅周波数特性（帯域消去中心周波数８Ｈｚ）
３９  ノッチフィルタの振幅周波数特性（帯域消去中心周波数１６Ｈｚ）
４０  ノッチフィルタの位相周波数特性（帯域消去中心周波数８Ｈｚ）
４１  ノッチフィルタの位相周波数特性（帯域消去中心周波数１６Ｈｚ）
４２  ノッチフィルタの位相差
４３  ノッチフィルタの振幅周波数特性
４４  ノッチフィルタの位相周波数特性
４５  位置指令速度成分ｄθｃｏｍ
５０  加算器
【図１】
2008004007.tif 000014
【図２】
2008004007.tif 000015
【図３】
2008004007.tif 000016
【図４】
2008004007.tif 000017
【図５】
2008004007.tif 000018
【図６】
2008004007.tif 000019
【図７】
2008004007.tif 000020
【図８】
2008004007.tif 000021
【図９】
2008004007.tif 000022
【図１０】
2008004007.tif 000023
【図１１】
2008004007.tif 000024
【図１２】
2008004007.tif 000025