JP5092831B2 - 位置決め制御装置 - Google Patents

Description

本発明は、モータ等の駆動装置を用いて位置決めを行う位置決め制御装置に関し、特に柔軟アームを有する産業用機械のアーム先端の振動を抑制する位置決め制御装置に関するものである。

柔軟アームを有する産業用機械の位置決め制御を行う従来の位置決め制御装置には、例えば特許文献１に開示されているものがある。
図５は、特許文献１で開示された発明の一実施形態に係る位置決め制御装置のブロック図である。この従来の位置決め制御装置は、Ｓ字位置指令発生部１０１と、２次フィルタ１０２と、減算器１０３と、フィードバック制御器１０４とから構成されている。また、伝達関数１０５は、フィードバック制御器１０４の出力であるトルク指令からモータ位置までの伝達関数Ｐ_M（ｓ）、伝達関数１０６はトルク指令から柔軟アーム先端位置までの伝達関数Ｐ_L（ｓ）である。

この従来の位置決め制御装置では、Ｓ字位置指令発生部１０１により生成された元の位置指令ｘ_rが２次フィルタ１０２を通過することによってフィードバック制御系の位置指令ｘ_rfが生成される。そして、フィードバックされたモータの位置ｘ_mと位置指令ｘ_rfとの偏差を減算器１０３によって演算し、フィードバック制御器１０４ではその偏差が最小になるようにトルク指令を生成する。

Ｓ字位置指令Ｘ_rおよびこのＳ字位置指令Ｘ_r（ｔ）の速度パターンＶ_r（ｔ）の時間関数を図６に示す。Ｓ字位置指令発生部１０１により発生されるＳ字形状の元の位置指令Ｘ_r（ｔ）を図６（ａ）に、速度パターンＶ_r（ｔ）を図６（ｂ）に示す。図６において、ｔ_aは加速時間、ｔ_dは加速開始から減速開始までの時間、ｔ_eは指令時間であり、そして、減速時間（ｔ_e−ｔ_d）と加速時間ｔ_aが等しくなっている。
ここで、柔軟アームの固有振動数がω_zで、柔軟アームの固有振動減衰係数がζ_zである場合は、２次フィルタ１０２の伝達関数

におけるパラメータを
ω₁＝ω_z （２）
および
ζ₁＝ζ_z （３）
および

を満たすように設定する。
図７には、２次フィルタ１０２を用いずに位置決め制御装置を駆動した場合のシミュレーション結果を示し、図８には、（２）〜（４）式の設定にて図５に示す従来の位置決め制御装置を駆動した場合のシミュレーション結果を示す。２次フィルタ１０２を用いない場合は、図７に示すように、モータの位置が目標の位置に到達しても負荷先端の位置は固有振動数ω_zで大きく振動し続ける。しかし、（１）式に示した２次フィルタ１０２を用いることで、図８に示すように、負荷先端の残留振動が抑制されていることがわかる。
特開２００４−２７２７４９号公報

近年、サーボシステムはディジタル制御されており、上述した位置決め制御装置もディジタル制御されることが一般的である。ここで、図５に示す位置決め制御装置をディジタル制御系の離散化位置決め制御装置として表すと図９に示すようになる。すなわち、積分要素１／ｓがＴ_s／（１−ｚ^-1）に変換され、微分要素ｓが（１−ｚ^-1）／Ｔ_sに変換されている。ただし、ｚはｚ変換演算子、Ｔ_sはサンプル周期である。

この離散化位置決め制御装置は、離散化Ｓ字位置指令発生部１と、離散化２次フィルタ２と、離散化減算器３と、離散化フィードバック制御器４とから構成されている。また伝達関数５は、離散化フィードバック制御器４の出力であるトルク指令からモータ位置までの伝達関数Ｐ_M（ｓ）、伝達関数６はトルク指令から柔軟アーム先端位置までの伝達関数Ｐ_L（ｓ）である。

この離散化位置決め制御装置では、離散化Ｓ字位置指令発生部１により生成された元の位置指令ｘ_zrが離散化２次フィルタ２を通過することによってフィードバック制御系の位置指令ｘ_zrfが生成される。そして、フィードバックされたモータの位置ｘ_mと位置指令ｘ_rfとの偏差を減算器３によって演算し、離散化フィードバック制御器４ではその偏差が最小になるようにトルク指令を生成する。
特に、離散化２次フィルタ２は次式で表される。

このシミュレーション結果を図１０、図１１に示す。図１０は離散化２次フィルタ２を用いない場合、図１１は離散化２次フィルタ２を用いた場合の結果である。
負荷先端の位置は、２次フィルタ２を用いることで連続系でシミュレーションした場合と同等に抑制しているが、図１１に示すように、指令トルク波形に高周波の振動が現れていることが確認できる。これは、（５）式に示す２次フィルタには２回の微分演算が含まれており、位置指令の波形が離散的（階段状）に変化するディジタル制御系では、その変化が指令トルクにまで現れるためである。
一般的に、モータの位置として、エンコーダのパルス量をフィードバックする場合、位置指令としてはパルス量を入力する。離散化Ｓ字位置指令発生部１の出力する位置指令の１サンプル周期当りの変化量をΔｘ_zrとした場合、離散化２次フィルタ２を通過した位置指令の１サンプル周期当りの変化量Δｘ_zrfは次式となる。

よって、離散化フィードバック制御器４の応答が十分に速いとするなら、上記の位置指令変化量Δｘ_zrに対する指令トルクの変化Δτは次式となる。ただし、Ｘ_Rはモータ１回転当りのエンコーダ分解能である。

つまり、位置指令ｘ_zrの１サンプル周期当りの変化量Δｘ_zrに比例してトルク微振動の振幅Δτも増加することがわかる。さらに、アームの固有振動数ω_zが低く、サンプル周期Ｔ_sが短くなるほど、指令トルクに現れる高周波の振動の振幅が大きくなる。
ここで、（７）式においてω₂の値を小さく設定することで、１サンプル周期当りのトルクリプルを小さくできるが、ω₂の値を（４）式以外の設定値で決定すると、離散化２次フィルタ２のＦ（ｚ）の出力が周波数ω₂で振動するため、最小でも（４）式におけるｋ＝１の場合に制約され、十分に高周波振動の抑制ができない。

このように離散系で表した場合、トルク指令値が振動的となり、機械系から大きな動作音が発生するなどの問題が生じる。特に、近年のマイコンの高性能化からサンプリング周期が短くなる傾向にあるため、この問題が顕著に現れることとなる。
本発明はこのような従来の課題を解決するためになされたものであり、その目的は、離散化され階段状に波形が変化する位置指令が入力された場合でも、負荷機械の位置決めすべき先端位置の低周波振動を抑制すると共に、指令トルクの高周波振動を抑制し、騒音の少ない位置決め制御を行うことを可能とする位置決め制御装置を提供することにある。

上記課題を解決するために、請求項１に係る位置決め制御装置は、比較的剛性の低い連結体で連結された負荷機械をモータにより駆動するディジタル制御された位置決め制御装置において、
任意形状の波形を有する位置指令を生成可能な位置指令発生部と、前記位置指令を前記負荷機械の固有振動をキャンセルするように補償して第１位置指令として出力する位置指令補償部と、前記第１位置指令の振動波形を平滑して第２位置指令として出力する平滑フィルタと、前記第２位置指令と前記負荷機械を駆動するモータの位置をフィードバックする信号とを入力してフィードバック制御を行うフィードバック制御部とを有し、
前記位置指令補償部の伝達関数Ｆ_d（ｚ）は、前記位置指令発生部の位置指令を入力して前記フィードバック制御部の位置指令として出力する２次フィルタの分子に相当する

により表され、前記伝達関数Ｆ _d （ｚ）におけるω ₁ を前記負荷機械の固有振動数ω _z とし、ζ ₁ を前記負荷機械の固有振動減衰係数ζ _z とし、Ｔ _s をサンプル周期として設定し、
前記負荷機械の先端位置を前記第２位置指令に追従させることを特徴とする。

上記のように構成された位置決め制御装置は、離散化され階段状に波形が変化する位置指令が入力された場合でも、負荷機械の先端位置の低周波振動を抑制すると共に、指令トルクの高周波振動を抑制することができ、騒音の少ない位置決め制御を行うことができる。
また、本願請求項１に係る位置決め制御装置は、平滑フィルタの伝達関数Ｆ _ｆ （ｚ）が、

により表され、伝達関数Ｆ_d（ｚ）におけるω₁を負荷機械の固有振動数ω_zとし、ζ₁を負荷機械の固有振動減衰係数ζ_zとし、Ｔ_sをサンプル周期とすることを特徴とする。

上記のように構成された位置決め制御装置は、フィードバック制御部に入力される第２位置指令の１サンプル周期当りの変化量を、２次以上のフィルタによって自由に制御できるため、離散化されて階段状に波形が変化する位置指令が入力された場合でも、指令トルクに現れる高周波振動を抑制しつつ、負荷機械の先端の低周波振動を抑制することができる。
また、請求項２に係る位置決め制御装置は、平滑フィルタの伝達関数Ｆ_ｆ（ｚ）が、

により表され、伝達関数Ｆ_f（ｚ）におけるω₃を次式で決定する事を特徴とする。

上記のように構成された位置決め制御装置は、負荷機械の固有振動数に応じてフィルタ時定数が変化するため、負荷機械の固有振動数によらず安定してトルク高周波振動を抑制することができる。
また、請求項３に係る位置決め制御装置は、請求項１に係る位置決め制御装置において、平滑フィルタの伝達関数Ｆ_f（ｚ）が

により表され、前記伝達関数Ｆ_f（ｚ）におけるＮを次式で計算される値に最も近い整数とする事を特徴とする。
Ｎ＝ｎ・ω_z・Ｔ_s （１≦ｎ：ｎは整数） （１２）
上記のように構成された位置決め制御装置は、請求項３に記載の平滑フィルタよりも小さい時定数でトルクの高周波振動を抑制することができる。

本発明に係る位置決め制御装置は、任意形状の波形を有する位置指令を生成可能な位置指令発生部と、前記位置指令を前記負荷機械の固有振動をキャンセルするように補償して第１位置指令として出力する位置指令補償部と、前記第１位置指令の振動波形を平滑して第２位置指令として出力する平滑フィルタと、前記第２位置指令と前記負荷機械を駆動するモータの位置をフィードバックする信号とを入力してフィードバック制御を行うフィードバック制御部とを有し、前記負荷機械の先端位置を前記第２位置指令に追従させるため、離散化され階段状に波形が変化する位置指令が入力された場合でも、負荷機械先端の低周波振動を抑制すると共に、指令トルクの高周波振動を抑制することができ、騒音の少ない位置決め制御を行うことが可能となる。

（第１の実施形態）
以下、本発明に係る位置決め制御装置の第１の実施形態について図面に基づいて説明する。
図１は、本発明の第１の実施形態の位置決め制御装置を示すブロック図である。図１において、図９中の構成要素と同一の構成要素には同一の番号を付し、説明を省略する。
本実施形態の位置決め制御装置は、Ｓ字形状の波形を有する位置指令だけでなく、任意形状の波形を有する位置指令を用いた場合でも、産業機械の柔軟アーム先端等の負荷機械先端の振動を抑制することができるようにし、また、位置指令の波形が離散的に変化する場合でも、指令トルクに現れる高周波の振動を抑制することができるようにしたものである。

本実施形態の位置決め制御装置における従来の位置決め制御装置と異なる構成は、図９における離散化Ｓ字位置指令発生部１を離散化任意位置指令発生部７に置き換え、離散化２次フィルタ２をＦ（ｚ）の分子に相当する離散化位置補償器８と離散化修正２次フィルタ９に置き換えたことである。
図１中の離散化位置補償器８の伝達関数Ｆ_d（ｚ）と離散化修正２次フィルタ９の伝達関数Ｆ_f（ｚ）は次式で表される。

上記の離散化修正２次フィルタ９を通過したときの位置指令ｘ_zrfの１サンプリング周期中の変化量Δｘ_zrfは次式となる。

上式において、ω₃を小さく設定すればΔｘ_zrfを小さくできることがわかり、Δｘ_zrfの大きさを任意に制御できることがわかる。例えば、離散化修正２次フィルタ９を通過した振動振幅Δｘ_zrfを離散化任意位置指令発生部７の出力に現れる振動振幅Δｘ_zr以下にする場合、ω₃は次式で決定すればよい。

上式においてｎ＝１とすれば、Δｘ_zrfとΔｘ_zrを同じにすることができる。
なお、（１３）式においてω₁、ζ₁は従来手法と同様にして、下記の式を満たすように設定する。
ω₁＝ω₂ （１７）
ζ₁＝ζ₂ （１８）
図１に示す位置決め制御装置において（１６）〜（１８）式でＦ_d（ｚ）とＦ_f（ｚ）のパラメータを決定した場合のシミュレーション結果を図２に示す。図１１に示した従来の位置決め制御装置の結果に対して、指令トルクに現れるトルク微振動の振幅が抑制され、アーム先端の振動は同等に抑制されていることが確認できる。また、トルク微振動の振幅Δｘ_zrfは離散化位置補償器８および離散化修正２次フィルタ９を用いない場合（図１０参照）と同等であることも確認できる。

このように、本実施形態に係る位置決め制御装置は、離散化フィードバック制御器４に入力される位置指令の１サンプル周期当りの変化量Δｘ_zrfを自由に制御できるため、離散化されて階段状に変化する位置指令が入力された場合でも、指令トルクに現れる高周波振動を抑制しつつ、負荷機械の先端の低周波振動を抑制することができる。

（第２の実施形態）
以下、本発明に係る位置決め制御装置の第２の実施形態について図面に基づいて説明する。図３は本発明の第２の実施形態の位置決め制御装置を示すブロック図である。図３において、図１中の構成要素と同一の構成要素には同一の番号を付し、説明を省略する。
本実施形態の位置決め制御装置が、従来の位置決め制御装置と異なる構成は、図１における離散化修正２次フィルタ９を移動平均フィルタ１０に置き換えたことである。移動平均フィルタ１０の伝達関数Ｆ_ma（ｚ）は次式となる。

（１９）式においてＮは移動平均の段数である。移動平均フィルタ１０を通過後に現れる位置指令の波形の微振動の振幅Δｘ_zrfは次式となる。

上式より、離散化任意位置指令発生部７の出力する位置指令ｘ_zrの１サンプルあたりの変化量Δｘ_zrに対して移動平均フィルタ通過後の位置指令の変化量Δｘ_zrfを小さくするには次のように移動平均フィルタの段数Ｎを決定すればよい。
Ｎ＝ｎ・ω_z・Ｔ_s （１≦ｎ：ｎは整数） （２１）
上式においてｎ＝１とすれば、Δｘ_zrfとΔｘ_zrを同じにすることができる。

図４に図３の構成でのシミュレーション結果を示す。指令トルクの微振動とアーム先端の振動とが共に抑制されている。また、アーム先端が目標位置まで達する時間が第１の実施例で示したシミュレーション結果（図２参照）と比べて短くなっている。これは、図１に示す離散化修正２次フィルタ９と図３に示す移動平均フィルタ１０とでは、微振動を抑制するための時定数が、移動平均フィルタ１０の方が小さくてすむためであり、その分全体的な遅れが小さくなったものである。
以上説明したように、離散化され階段状に変化する任意形状の波形を有する位置指令が入力された場合でも、位置決め制御装置は、柔軟アーム先端の低周波振動を抑制すると共に、指令トルクの高周波振動を抑制することができ、騒音の少ない位置決め制御を行うことが可能となる。

産業用機械のアーム先端の位置決めを始めとする、モータ等の駆動装置を用いた位置決めを行う際に、振動を抑制して騒音の少ない位置決め制御を行うことができる。

本発明の第１の実施形態の位置決め制御装置を示すブロック図である。 同実施形態の位置決め制御装置を用いた場合のシミュレーション結果を示す図である。 本発明の第２の実施形態の位置決め制御装置を示すブロック図である。 同実施形態の位置決め制御装置を用いた場合のシミュレーション結果を示す図である。 従来の位置決め制御装置の構成を示すブロック図である。 Ｓ字位置指令および、このＳ字位置指令の速度パターンの時間関数を示す図である。 図５に示す従来の位置決め制御装置において、２次フィルタを用いない場合のシミュレーション結果を示す図である。 図５に示す従来の位置決め制御装置において、２次フィルタを用いた場合のシミュレーション結果を示す図である。 離散系で表した、従来の離散化位置決め制御装置の構成を示すブロック図である。 図９に示す従来の離散化位置決め制御装置において、離散化２次フィルタを用いない場合のシミュレーション結果を示す図である。 図９に示す従来の離散化位置決め制御装置において、離散化２次フィルタを用いた場合のシミュレーション結果を示す図である。

符号の説明

１ 離散化Ｓ字位置指令発生部
２ 離散化２次フィルタ
３ 離散化減算器
４ 離散化フィードバック制御器
５ トルク指令からモータ位置までの伝達関数
６ トルク指令から負荷先端までの伝達関数
７ 離散化任意位置指令発生部
８ 離散化位置補償器
９ 離散化修正２次フィルタ
１０ 移動平均フィルタ
１０１ Ｓ字位置指令発生部
１０２ ２次フィルタ
１０３ 減算器
１０４ フィードバック制御器
１０５ トルク指令からモータ位置までの伝達関数
１０６ トルク指令から負荷先端までの伝達関数

Claims (3)

1. 比較的剛性の低い連結体で連結された負荷機械をモータにより駆動するディジタル制御された位置決め制御装置において、
   任意形状の波形を有する位置指令を生成可能な位置指令発生部と、前記位置指令を前記負荷機械の固有振動をキャンセルするように補償して第１位置指令として出力する位置指令補償部と、前記第１位置指令の振動波形を平滑して第２位置指令として出力する平滑フィルタと、前記第２位置指令と前記負荷機械を駆動するモータの位置をフィードバックする信号とを入力してフィードバック制御を行うフィードバック制御部とを有し、
   前記位置指令補償部の伝達関数Ｆ_d（ｚ）は、前記位置指令発生部の位置指令を入力して前記フィードバック制御部の位置指令として出力する２次フィルタの分子に相当する
   

   

   により表され、前記伝達関数Ｆ _d （ｚ）におけるω ₁ を前記負荷機械の固有振動数ω _z とし、ζ ₁ を前記負荷機械の固有振動減衰係数ζ _z とし、Ｔ _s をサンプル周期として設定し、
   前記負荷機械の先端位置を前記第２位置指令に追従させることを特徴とする位置決め制御装置。
2. 前記平滑フィルタの伝達関数Ｆ_f（ｚ）は、
   

   

   により表され、
   前記伝達関数Ｆ_f（ｚ）におけるω₃を
   

   

   で決定する事を特徴とする請求項１に記載の位置決め制御装置。
3. 前記平滑フィルタの伝達関数Ｆ_f（ｚ）が
   

   

   により表され、前記伝達関数Ｆ_f（ｚ）におけるＮを、
   Ｎ＝ｎ・ω_z・Ｔ_s （１≦ｎ：ｎは整数）
   で計算される値に最も近い整数とする事を特徴とする請求項１に記載の位置決め制御装置。

Images