(57)【特許請求の範囲】
【請求項１】
比較的剛性の低い連結体で連結された負荷機械をモータにより駆動するディジタル制御された位置決め制御装置において、
任意形状の波形を有する位置指令を生成可能な位置指令発生部と、前記位置指令を前記負荷機械の固有振動をキャンセルするように補償して第１位置指令として出力する位置指令補償部と、前記第１位置指令の振動波形を平滑して第２位置指令として出力する平滑フィルタと、前記第２位置指令と前記負荷機械を駆動するモータの位置をフィードバックする信号とを入力してフィードバック制御を行うフィードバック制御部とを有し、
前記位置指令補償部の伝達関数Ｆd（ｚ）は、前記位置指令発生部の位置指令を入力して前記フィードバック制御部の位置指令として出力する２次フィルタの分子に相当する
【数１】
0005092831.tif 000017
により表され、前記伝達関数Ｆd（ｚ）におけるω1を前記負荷機械の固有振動数ωzとし、ζ1を前記負荷機械の固有振動減衰係数ζzとし、Ｔsをサンプル周期として設定し、
前記負荷機械の先端位置を前記第２位置指令に追従させることを特徴とする位置決め制御装置。
【請求項２】
前記平滑フィルタの伝達関数Ｆf（ｚ）は、
【数２】
0005092831.tif 000018
により表され、
前記伝達関数Ｆf（ｚ）におけるω3を
【数３】
0005092831.tif 000019
で決定する事を特徴とする請求項１に記載の位置決め制御装置。
【請求項３】
前記平滑フィルタの伝達関数Ｆf（ｚ）が
【数４】
0005092831.tif 000020
により表され、前記伝達関数Ｆf（ｚ）におけるＮを、
Ｎ＝ｎ・ωz・Ｔs （１≦ｎ：ｎは整数）
で計算される値に最も近い整数とする事を特徴とする請求項１に記載の位置決め制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、モータ等の駆動装置を用いて位置決めを行う位置決め制御装置に関し、特に柔軟アームを有する産業用機械のアーム先端の振動を抑制する位置決め制御装置に関するものである。
【背景技術】
【０００２】
柔軟アームを有する産業用機械の位置決め制御を行う従来の位置決め制御装置には、例えば特許文献１に開示されているものがある。
図５は、特許文献１で開示された発明の一実施形態に係る位置決め制御装置のブロック図である。この従来の位置決め制御装置は、Ｓ字位置指令発生部１０１と、２次フィルタ１０２と、減算器１０３と、フィードバック制御器１０４とから構成されている。また、伝達関数１０５は、フィードバック制御器１０４の出力であるトルク指令からモータ位置までの伝達関数ＰM（ｓ）、伝達関数１０６はトルク指令から柔軟アーム先端位置までの伝達関数ＰL（ｓ）である。
【０００３】
この従来の位置決め制御装置では、Ｓ字位置指令発生部１０１により生成された元の位置指令ｘrが２次フィルタ１０２を通過することによってフィードバック制御系の位置指令ｘrfが生成される。そして、フィードバックされたモータの位置ｘmと位置指令ｘrfとの偏差を減算器１０３によって演算し、フィードバック制御器１０４ではその偏差が最小になるようにトルク指令を生成する。
【０００４】
Ｓ字位置指令ＸrおよびこのＳ字位置指令Ｘr（ｔ）の速度パターンＶr（ｔ）の時間関数を図６に示す。Ｓ字位置指令発生部１０１により発生されるＳ字形状の元の位置指令Ｘr（ｔ）を図６（ａ）に、速度パターンＶr（ｔ）を図６（ｂ）に示す。図６において、ｔaは加速時間、ｔdは加速開始から減速開始までの時間、ｔeは指令時間であり、そして、減速時間（ｔe−ｔd）と加速時間ｔaが等しくなっている。
ここで、柔軟アームの固有振動数がωzで、柔軟アームの固有振動減衰係数がζzである場合は、２次フィルタ１０２の伝達関数
【０００５】
【数５】
0005092831.tif 000002
におけるパラメータを
ω1＝ωz                                                      （２）
および
ζ1＝ζz                                                      （３）
および
【０００６】
【数６】
0005092831.tif 000003
を満たすように設定する。
図７には、２次フィルタ１０２を用いずに位置決め制御装置を駆動した場合のシミュレーション結果を示し、図８には、（２）〜（４）式の設定にて図５に示す従来の位置決め制御装置を駆動した場合のシミュレーション結果を示す。２次フィルタ１０２を用いない場合は、図７に示すように、モータの位置が目標の位置に到達しても負荷先端の位置は固有振動数ωzで大きく振動し続ける。しかし、（１）式に示した２次フィルタ１０２を用いることで、図８に示すように、負荷先端の残留振動が抑制されていることがわかる。
【特許文献１】特開２００４−２７２７４９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
近年、サーボシステムはディジタル制御されており、上述した位置決め制御装置もディジタル制御されることが一般的である。ここで、図５に示す位置決め制御装置をディジタル制御系の離散化位置決め制御装置として表すと図９に示すようになる。すなわち、積分要素１／ｓがＴs／（１−ｚ-1）に変換され、微分要素ｓが（１−ｚ-1）／Ｔsに変換されている。ただし、ｚはｚ変換演算子、Ｔsはサンプル周期である。
【０００８】
この離散化位置決め制御装置は、離散化Ｓ字位置指令発生部１と、離散化２次フィルタ２と、離散化減算器３と、離散化フィードバック制御器４とから構成されている。また伝達関数５は、離散化フィードバック制御器４の出力であるトルク指令からモータ位置までの伝達関数ＰM（ｓ）、伝達関数６はトルク指令から柔軟アーム先端位置までの伝達関数ＰL（ｓ）である。
【０００９】
この離散化位置決め制御装置では、離散化Ｓ字位置指令発生部１により生成された元の位置指令ｘzrが離散化２次フィルタ２を通過することによってフィードバック制御系の位置指令ｘzrfが生成される。そして、フィードバックされたモータの位置ｘmと位置指令ｘrfとの偏差を減算器３によって演算し、離散化フィードバック制御器４ではその偏差が最小になるようにトルク指令を生成する。
特に、離散化２次フィルタ２は次式で表される。
【００１０】
【数７】
0005092831.tif 000004
【００１１】
このシミュレーション結果を図１０、図１１に示す。図１０は離散化２次フィルタ２を用いない場合、図１１は離散化２次フィルタ２を用いた場合の結果である。
負荷先端の位置は、２次フィルタ２を用いることで連続系でシミュレーションした場合と同等に抑制しているが、図１１に示すように、指令トルク波形に高周波の振動が現れていることが確認できる。これは、（５）式に示す２次フィルタには２回の微分演算が含まれており、位置指令の波形が離散的（階段状）に変化するディジタル制御系では、その変化が指令トルクにまで現れるためである。
一般的に、モータの位置として、エンコーダのパルス量をフィードバックする場合、位置指令としてはパルス量を入力する。離散化Ｓ字位置指令発生部１の出力する位置指令の１サンプル周期当りの変化量をΔｘzrとした場合、離散化２次フィルタ２を通過した位置指令の１サンプル周期当りの変化量Δｘzrfは次式となる。
【００１２】
【数８】
0005092831.tif 000005
【００１３】
よって、離散化フィードバック制御器４の応答が十分に速いとするなら、上記の位置指令変化量Δｘzrに対する指令トルクの変化Δτは次式となる。ただし、ＸRはモータ１回転当りのエンコーダ分解能である。
【００１４】
【数９】
0005092831.tif 000006
【００１５】
つまり、位置指令ｘzrの１サンプル周期当りの変化量Δｘzrに比例してトルク微振動の振幅Δτも増加することがわかる。さらに、アームの固有振動数ωzが低く、サンプル周期Ｔsが短くなるほど、指令トルクに現れる高周波の振動の振幅が大きくなる。
ここで、（７）式においてω2の値を小さく設定することで、１サンプル周期当りのトルクリプルを小さくできるが、ω2の値を（４）式以外の設定値で決定すると、離散化２次フィルタ２のＦ（ｚ）の出力が周波数ω2で振動するため、最小でも（４）式におけるｋ＝１の場合に制約され、十分に高周波振動の抑制ができない。
【００１６】
このように離散系で表した場合、トルク指令値が振動的となり、機械系から大きな動作音が発生するなどの問題が生じる。特に、近年のマイコンの高性能化からサンプリング周期が短くなる傾向にあるため、この問題が顕著に現れることとなる。
本発明はこのような従来の課題を解決するためになされたものであり、その目的は、離散化され階段状に波形が変化する位置指令が入力された場合でも、負荷機械の位置決めすべき先端位置の低周波振動を抑制すると共に、指令トルクの高周波振動を抑制し、騒音の少ない位置決め制御を行うことを可能とする位置決め制御装置を提供することにある。
【課題を解決するための手段】
【００１７】
上記課題を解決するために、請求項１に係る位置決め制御装置は、比較的剛性の低い連結体で連結された負荷機械をモータにより駆動するディジタル制御された位置決め制御装置において、
任意形状の波形を有する位置指令を生成可能な位置指令発生部と、前記位置指令を前記負荷機械の固有振動をキャンセルするように補償して第１位置指令として出力する位置指令補償部と、前記第１位置指令の振動波形を平滑して第２位置指令として出力する平滑フィルタと、前記第２位置指令と前記負荷機械を駆動するモータの位置をフィードバックする信号とを入力してフィードバック制御を行うフィードバック制御部とを有し、
前記位置指令補償部の伝達関数Ｆd（ｚ）は、前記位置指令発生部の位置指令を入力して前記フィードバック制御部の位置指令として出力する２次フィルタの分子に相当する
【数１】
0005092831.tif 000007
により表され、前記伝達関数Ｆd（ｚ）におけるω1を前記負荷機械の固有振動数ωzとし、ζ1を前記負荷機械の固有振動減衰係数ζzとし、Ｔsをサンプル周期として設定し、
前記負荷機械の先端位置を前記第２位置指令に追従させることを特徴とする。
【００１８】
上記のように構成された位置決め制御装置は、離散化され階段状に波形が変化する位置指令が入力された場合でも、負荷機械の先端位置の低周波振動を抑制すると共に、指令トルクの高周波振動を抑制することができ、騒音の少ない位置決め制御を行うことができる。
また、本願請求項１に係る位置決め制御装置は、平滑フィルタの伝達関数Ｆｆ（ｚ）が、
【００１９】
【数１０】
0005092831.tif 000008
により表され、伝達関数Ｆd（ｚ）におけるω1を負荷機械の固有振動数ωzとし、ζ1を負荷機械の固有振動減衰係数ζzとし、Ｔsをサンプル周期とすることを特徴とする。
【００２０】
上記のように構成された位置決め制御装置は、フィードバック制御部に入力される第２位置指令の１サンプル周期当りの変化量を、２次以上のフィルタによって自由に制御できるため、離散化されて階段状に波形が変化する位置指令が入力された場合でも、指令トルクに現れる高周波振動を抑制しつつ、負荷機械の先端の低周波振動を抑制することができる。
また、請求項２に係る位置決め制御装置は、平滑フィルタの伝達関数Ｆｆ（ｚ）が、
【００２１】
【数１１】
0005092831.tif 000009
により表され、伝達関数Ｆf（ｚ）におけるω3を次式で決定する事を特徴とする。
【００２２】
【数１２】
0005092831.tif 000010
【００２３】
上記のように構成された位置決め制御装置は、負荷機械の固有振動数に応じてフィルタ時定数が変化するため、負荷機械の固有振動数によらず安定してトルク高周波振動を抑制することができる。
また、請求項３に係る位置決め制御装置は、請求項１に係る位置決め制御装置において、平滑フィルタの伝達関数Ｆf（ｚ）が
【００２４】
【数１３】
0005092831.tif 000011
により表され、前記伝達関数Ｆf（ｚ）におけるＮを次式で計算される値に最も近い整数とする事を特徴とする。
Ｎ＝ｎ・ωz・Ｔs                      （１≦ｎ：ｎは整数）          （１２）
上記のように構成された位置決め制御装置は、請求項３に記載の平滑フィルタよりも小さい時定数でトルクの高周波振動を抑制することができる。
【発明の効果】
【００２５】
本発明に係る位置決め制御装置は、任意形状の波形を有する位置指令を生成可能な位置指令発生部と、前記位置指令を前記負荷機械の固有振動をキャンセルするように補償して第１位置指令として出力する位置指令補償部と、前記第１位置指令の振動波形を平滑して第２位置指令として出力する平滑フィルタと、前記第２位置指令と前記負荷機械を駆動するモータの位置をフィードバックする信号とを入力してフィードバック制御を行うフィードバック制御部とを有し、前記負荷機械の先端位置を前記第２位置指令に追従させるため、離散化され階段状に波形が変化する位置指令が入力された場合でも、負荷機械先端の低周波振動を抑制すると共に、指令トルクの高周波振動を抑制することができ、騒音の少ない位置決め制御を行うことが可能となる。
【発明を実施するための最良の形態】
【００２６】
（第１の実施形態）
以下、本発明に係る位置決め制御装置の第１の実施形態について図面に基づいて説明する。
図１は、本発明の第１の実施形態の位置決め制御装置を示すブロック図である。図１において、図９中の構成要素と同一の構成要素には同一の番号を付し、説明を省略する。
本実施形態の位置決め制御装置は、Ｓ字形状の波形を有する位置指令だけでなく、任意形状の波形を有する位置指令を用いた場合でも、産業機械の柔軟アーム先端等の負荷機械先端の振動を抑制することができるようにし、また、位置指令の波形が離散的に変化する場合でも、指令トルクに現れる高周波の振動を抑制することができるようにしたものである。
【００２７】
本実施形態の位置決め制御装置における従来の位置決め制御装置と異なる構成は、図９における離散化Ｓ字位置指令発生部１を離散化任意位置指令発生部７に置き換え、離散化２次フィルタ２をＦ（ｚ）の分子に相当する離散化位置補償器８と離散化修正２次フィルタ９に置き換えたことである。
図１中の離散化位置補償器８の伝達関数Ｆd（ｚ）と離散化修正２次フィルタ９の伝達関数Ｆf（ｚ）は次式で表される。
【００２８】
【数１４】
0005092831.tif 000012
【００２９】
上記の離散化修正２次フィルタ９を通過したときの位置指令ｘzrfの１サンプリング周期中の変化量Δｘzrfは次式となる。
【００３０】
【数１５】
0005092831.tif 000013
【００３１】
上式において、ω3を小さく設定すればΔｘzrfを小さくできることがわかり、Δｘzrfの大きさを任意に制御できることがわかる。例えば、離散化修正２次フィルタ９を通過した振動振幅Δｘzrfを離散化任意位置指令発生部７の出力に現れる振動振幅Δｘzr以下にする場合、ω3は次式で決定すればよい。
【００３２】
【数１６】
0005092831.tif 000014
【００３３】
上式においてｎ＝１とすれば、ΔｘzrfとΔｘzrを同じにすることができる。
なお、（１３）式においてω1、ζ1は従来手法と同様にして、下記の式を満たすように設定する。
ω1＝ω2                                                        （１７）
ζ1＝ζ2                                                        （１８）
図１に示す位置決め制御装置において（１６）〜（１８）式でＦd（ｚ）とＦf（ｚ）のパラメータを決定した場合のシミュレーション結果を図２に示す。図１１に示した従来の位置決め制御装置の結果に対して、指令トルクに現れるトルク微振動の振幅が抑制され、アーム先端の振動は同等に抑制されていることが確認できる。また、トルク微振動の振幅Δｘzrfは離散化位置補償器８および離散化修正２次フィルタ９を用いない場合（図１０参照）と同等であることも確認できる。
【００３４】
このように、本実施形態に係る位置決め制御装置は、離散化フィードバック制御器４に入力される位置指令の１サンプル周期当りの変化量Δｘzrfを自由に制御できるため、離散化されて階段状に変化する位置指令が入力された場合でも、指令トルクに現れる高周波振動を抑制しつつ、負荷機械の先端の低周波振動を抑制することができる。
【００３５】
（第２の実施形態）
以下、本発明に係る位置決め制御装置の第２の実施形態について図面に基づいて説明する。図３は本発明の第２の実施形態の位置決め制御装置を示すブロック図である。図３において、図１中の構成要素と同一の構成要素には同一の番号を付し、説明を省略する。
本実施形態の位置決め制御装置が、従来の位置決め制御装置と異なる構成は、図１における離散化修正２次フィルタ９を移動平均フィルタ１０に置き換えたことである。移動平均フィルタ１０の伝達関数Ｆma（ｚ）は次式となる。
【００３６】
【数１７】
0005092831.tif 000015
【００３７】
（１９）式においてＮは移動平均の段数である。移動平均フィルタ１０を通過後に現れる位置指令の波形の微振動の振幅Δｘzrfは次式となる。
【００３８】
【数１８】
0005092831.tif 000016
【００３９】
上式より、離散化任意位置指令発生部７の出力する位置指令ｘzrの１サンプルあたりの変化量Δｘzrに対して移動平均フィルタ通過後の位置指令の変化量Δｘzrfを小さくするには次のように移動平均フィルタの段数Ｎを決定すればよい。
Ｎ＝ｎ・ωz・Ｔs            （１≦ｎ：ｎは整数）                    （２１）
上式においてｎ＝１とすれば、ΔｘzrfとΔｘzrを同じにすることができる。
【００４０】
図４に図３の構成でのシミュレーション結果を示す。指令トルクの微振動とアーム先端の振動とが共に抑制されている。また、アーム先端が目標位置まで達する時間が第１の実施例で示したシミュレーション結果（図２参照）と比べて短くなっている。これは、図１に示す離散化修正２次フィルタ９と図３に示す移動平均フィルタ１０とでは、微振動を抑制するための時定数が、移動平均フィルタ１０の方が小さくてすむためであり、その分全体的な遅れが小さくなったものである。
以上説明したように、離散化され階段状に変化する任意形状の波形を有する位置指令が入力された場合でも、位置決め制御装置は、柔軟アーム先端の低周波振動を抑制すると共に、指令トルクの高周波振動を抑制することができ、騒音の少ない位置決め制御を行うことが可能となる。
【産業上の利用可能性】
【００４１】
産業用機械のアーム先端の位置決めを始めとする、モータ等の駆動装置を用いた位置決めを行う際に、振動を抑制して騒音の少ない位置決め制御を行うことができる。
【図面の簡単な説明】
【００４２】
【図１】本発明の第１の実施形態の位置決め制御装置を示すブロック図である。
【図２】同実施形態の位置決め制御装置を用いた場合のシミュレーション結果を示す図である。
【図３】本発明の第２の実施形態の位置決め制御装置を示すブロック図である。
【図４】同実施形態の位置決め制御装置を用いた場合のシミュレーション結果を示す図である。
【図５】従来の位置決め制御装置の構成を示すブロック図である。
【図６】Ｓ字位置指令および、このＳ字位置指令の速度パターンの時間関数を示す図である。
【図７】図５に示す従来の位置決め制御装置において、２次フィルタを用いない場合のシミュレーション結果を示す図である。
【図８】図５に示す従来の位置決め制御装置において、２次フィルタを用いた場合のシミュレーション結果を示す図である。
【図９】離散系で表した、従来の離散化位置決め制御装置の構成を示すブロック図である。
【図１０】図９に示す従来の離散化位置決め制御装置において、離散化２次フィルタを用いない場合のシミュレーション結果を示す図である。
【図１１】図９に示す従来の離散化位置決め制御装置において、離散化２次フィルタを用いた場合のシミュレーション結果を示す図である。
【符号の説明】
【００４３】
１      離散化Ｓ字位置指令発生部
２      離散化２次フィルタ
３      離散化減算器
４      離散化フィードバック制御器
５      トルク指令からモータ位置までの伝達関数
６      トルク指令から負荷先端までの伝達関数
７      離散化任意位置指令発生部
８      離散化位置補償器
９      離散化修正２次フィルタ
１０    移動平均フィルタ
１０１  Ｓ字位置指令発生部
１０２  ２次フィルタ
１０３  減算器
１０４  フィードバック制御器
１０５  トルク指令からモータ位置までの伝達関数
１０６  トルク指令から負荷先端までの伝達関数
【図１】
0005092831.tif 000021
【図２】
0005092831.tif 000022
【図３】
0005092831.tif 000023
【図４】
0005092831.tif 000024
【図５】
0005092831.tif 000025
【図６】
0005092831.tif 000026
【図７】
0005092831.tif 000027
【図８】
0005092831.tif 000028
【図９】
0005092831.tif 000029
【図１０】
0005092831.tif 000030
【図１１】
0005092831.tif 000031