(57)【特許請求の範囲】
【請求項１】
任意形状の位置指令を生成可能な任意位置指令発生部と、
前記任意位置指令発生部により生成された位置指令を入力してフィードバック系の位置指令として出力するＣＩＣフィルタと、
前記ＣＩＣフィルタからのフィードバック系の位置指令および負荷機械を駆動するモータの位置信号を入力してフィードバック制御を行うフィードバック制御部と、
を有し、
柔軟アームを含む負荷機械をモータにより駆動して、前記柔軟アームに連結された負荷先端の位置を前記ＣＩＣフィルタから出力されたフィードバック系の位置指令に追従させる位置決め制御装置において、
前記ＣＩＣフィルタの伝達関数Ｆ2（ｓ）が、
【数１】
0004340846.tif 000028
により表され、前記伝達関数Ｆ2（ｓ）中における、ｍが２以上の自然数に設定され、前記伝達関数Ｆ2（ｓ）中における係数Ｂ1、Ｂ2、・・・、Ｂmおよび時定数Ｔ1、Ｔ2、・・・、Ｔmを、前記元の位置指令と前記ＣＩＣフィルタの出力の定常偏差が０となるような無定常偏差条件および前記負荷機械の振動極をキャンセルできるような振動抑制条件を満たすように設定されていることを特徴とする位置決め制御装置。
【請求項２】
ｚをｚ変換演算子、Ｔをサンプル周期とした場合、前記伝達関数Ｆ2（ｓ）における積分要素１／ｓがＴ／（１−ｚ-1）に変換され、すべてのｉにおいてｎiをＴi／Ｔの最も近い整数とした場合、前記伝達関数Ｆ2（ｓ）における遅れ要素ｅ-Tisがｚ-niに変換され、前記伝達関数Ｆ2（ｓ）における係数Ｂ1、Ｂ2、・・・、Ｂmのうちの任意の２つ係数が前記元の位置指令と離散化ＣＩＣフィルタの出力の定常偏差が０となるような離散化無定常偏差条件を満たすように設定されている、請求項１記載の位置決め制御装置。
【発明の詳細な説明】
【発明の属する技術分野】
本発明は、モータで負荷を駆動して位置決めを行う位置決め制御装置に関し、特に、柔軟アームを介して負荷テーブルに連結される負荷先端に存在する振動を抑制する技術に関する。
【従来の技術】
近年、工業用ロボットは作業の高速化、軽量化、小型化等の要求から、そのアームを軽量化することが行われている。しかし、アームを軽量化するとその剛性不足のために振動が発生し易くなるという問題が発生する。このような剛性の低い柔軟アームを有する負荷を制御する場合、柔軟アームの先端に接続された負荷先端の振動を抑制しつつ、高速高精度のサーボ制御を行うことが求められる。
柔軟アームを有する産業用機械の位置決め制御を行う従来の位置決め制御装置の構成を図４に示す。制御の対象となる産業用機械の機構は、モータ５と、負荷テーブル８と、負荷先端１０と、負荷テーブル８と負荷先端１０を連結する柔軟アーム９と、モータ５と負荷テーブル８を連結するトルク伝達機構７とを備えている。この従来の位置決め制御装置は、位置指令発生部１と、プレフィルタ２と、フィードバック制御部３と、サーボドライバ４と、エンコーダ６とから構成されている。
この従来の位置決め制御装置では、位置指令発生部１で生成された元の位置指令Ｘrがプレフィルタ２を通過することによってフィードバック制御系の位置指令Ｘrfが生成される。そして、モータ５に繋げられたエンコーダ６からのモータ位置信号Ｘmはフィードバック制御部３にフィードバックされ、フィードバック制御部３では、フィードバック制御系の位置指令Ｘrfおよびモータ位置信号Ｘmを用いてフィードバック制御が行われる。そして、フィードバック制御部３の出力がサーボドライバ４に入力されることによりモータ５の駆動が行われる。
一般に、モータ５と負荷テーブル８を連結するトルク伝達機構７の剛性は高いので、フィードバック制御部３のゲインを十分大きく上げることができる。そのため、負荷テーブル８の位置またはモータ位置信号Ｘmをほぼフィードバック位置指令Ｘrfに追従させることができる。しかし、負荷テーブル８と負荷先端１０を連結する柔軟アーム９の剛性が低いことにより、負荷テーブル８は目標位置に到達しても、負荷先端１０は低周波数で大きく振動し続けるという問題が発生する。
このような従来の位置決め制御装置のシミュレーション結果を図５に示す。図５を参照すると、モータ位置信号Ｘmは、ほぼ元の位置指令Ｘrに追従しているのに対して、負荷先端位置Ｘhは振動してしまっていることがわかる。
従来は、このような振動を抑制するために図４に示した構成において、位置指令発生部１をＳ字位置指令発生部に置き換え、プレフィルタ２を２次フィルタにより実現した位置決め制御装置が提案されている（例えば、非特許文献１参照。）。
このような従来の位置決め制御装置を図６に示す。尚、この図６では、伝達関数１１は、図４におけるフィードバック制御系の位置指令Ｘrfからモータ位置Ｘmまでの伝達関数、伝達関数１２は、図４におけるモータ位置Ｘmから負荷先端位置Ｘhまでの伝達関数である。
この従来の位置決め制御装置では、Ｓ字位置指令発生部１３により生成された元の位置指令Ｘrが２次フィルタ１８を通過することによってフィードバック制御系の位置指令Ｘrfが生成される。
Ｓ字位置指令Ｘrおよび、このＳ字位置指令Ｘrの速度パターンＶr（ｔ）および加速度パターンＡr（ｔ）の時間関数を図７に示す。Ｓ字位置指令発生部１３により生成されるＳ字形状の元の位置指令Ｘrを図７（ａ）に示す。また、このＳ字形状の元の位置指令Ｘrの速度パターンＶr（ｔ）を図７（ｂ）に示し、加速度パターンＡr（ｔ）を図７（ｃ）に示す。図７（ａ）〜図７（ｃ）において、
Ｖr（ｔ）＝
【数１】
0004340846.tif 000002
（Ｘr（ｔ）の微分）、Ａr（ｔ）＝
【数２】
0004340846.tif 000003
（Ｖr（ｔ）の微分）、ｔaは加速時間、ｔdは加速開始から減速開始までの時間、ｔeは指令時間であり、そして、減速時間（ｔe−ｔd）と加速時間ｔaが等しくなっている。
ここで、負荷テーブル８、柔軟アーム９および負荷先端１０を含む負荷機械の固有角振動数がω0である場合は、２次フィルタ１８の伝達関数
【数３】
0004340846.tif 000004
におけるパラメータを
ω1＝ω0 （２）
および
【数４】
0004340846.tif 000005
を満たすように設定する。このように設定した場合における、図６に示した従来の位置決め制御装置を用いた場合のシミュレーション結果を図８に示す。
図８を参照すると、２次フィルタ１８を通過した後のフィードバック系の位置指令Ｘrfは元の位置指令Ｘrと較べて指令時間が長く伸びてはおらず、また位置決め完了後の負荷先端の残留振動が図５と比較して抑えられていることがわかる。
また、フィードバック制御系の位置指令が目標指令と一致したときにプレフィルタの出力をゼロとすることにより、フィードバック制御系の位置指令を目標指令に定着させて、短時間に安定な位置決めを行うようにした位置決め制御装置も提案されている（例えば、特許文献１参照）。
しかし、上述した従来の位置決め制御装置では、負荷機械の固有振動の減衰を考慮することなくプレフィルタを構成しているため、制御系の極零点キャンセルできず、残留振動を十分に抑制することができなかった。また、元の位置指令がＳ字形状であることを前提としているため、元の位置指令がＳ字形状でないと適応できなかった。
【特許文献１】
特開２００２−２２９６０２号公報
【非特許文献１】
「電気学会研究会資料」ＩＩＣ−９６−１７， ｐｐ．７５−８４
【発明が解決しようとする課題】
上述した従来の位置決め制御装置では、負荷機械の固有振動の減衰がある場合に対して、制御系の極零点キャンセルできず、負荷先端の残留振動を十分に抑え切れなく、また、元の位置指令がＳ字形状でないと適応できないという問題点があった。
そこで、本発明は上記のような問題を解決するためになされたものであり、その目的は、元の位置指令がＳ字形状でない場合にも、負荷機械の固有振動の減衰を考慮して負荷先端の残留振動を十分に抑制し、高速高精度位置決め制御を実現することのできる位置決め制御装置を提供することである。
【課題を解決するための手段】
上記目的を達成するために、本発明の位置決め制御装置は、
任意形状の位置指令を生成可能な任意位置指令発生部と、
前記任意位置指令発生部により生成された位置指令を入力してフィードバック系の位置指令として出力するＣＩＣフィルタと、
前記ＣＩＣフィルタからのフィードバック系の位置指令および負荷機械を駆動するモータの位置信号を入力してフィードバック制御を行うフィードバック制御部と、
を有し、
柔軟アームを含む負荷機械をモータにより駆動して、前記柔軟アームに連結された負荷先端の位置を前記ＣＩＣフィルタから出力されたフィードバック系の位置指令に追従させる位置決め制御装置において、
前記ＣＩＣフィルタの伝達関数Ｆ2（ｓ）が、
【数５】
0004340846.tif 000006
により表され、前記伝達関数Ｆ2（ｓ）中における、ｍが２以上の自然数に設定され、前記伝達関数Ｆ2（ｓ）中における係数Ｂ1、Ｂ2、・・・、Ｂmおよび時定数Ｔ1、Ｔ2、・・・、Ｔmを、前記元の位置指令と前記ＣＩＣフィルタの出力の定常偏差が０となるような無定常偏差条件および前記負荷機械の振動極をキャンセルできるような振動抑制条件を満たすように設定されていることを特徴とする。
本発明によれば、プレフィルタとしてＣＩＣフィルタを用い、このＣＩＣフィルタの伝達関数における係数および時定数を、元の位置指令とＣＩＣフィルタの出力の定常偏差が０となるような無定常偏差条件および負荷機械の振動極をキャンセルできるような振動抑制条件を満たすように設定するようにしているので、任意形状の元の位置指令に対しても、制御系は定常偏差が残らず負荷先端の残留振動を抑制できる。
さらに、本発明の他の位置決め制御装置では、ｚをｚ変換演算子、Ｔをサンプル周期とした場合、前記伝達関数Ｆ2（ｓ）における積分要素１／ｓがＴ／（１−ｚ-1）に変換し、すべてのｉにおいてｎiをＴi／Ｔの最も近い整数とした場合、前記伝達関数Ｆ2（ｓ）における遅れ要素ｅ-Tisがｚ-niに変換し、前記伝達関数Ｆ2（ｓ）における係数Ｂ1、Ｂ2、・・・、Ｂmのうちの任意の２つ係数が前記元の位置指令と離散化ＣＩＣフィルタの出力の定常偏差が０となるような離散化無定常偏差条件を満たすように設定するようにしてもよい。
本発明は、ＣＩＣフィルタの伝達関数を離散系により表現するようにしたものである。
【発明の実施の形態】
次に、本発明の実施の形態について図面を参照して詳細に説明する。
（第１の実施形態）
次に、本発明の第１の実施形態の位置決め制御装置について説明する。図１は本発明の第１の実施形態の位置決め制御装置の構成を示すブロック図である。図１において、図６中の構成要素と同一の構成要素には同一の符号を付し、説明を省略するものとする。
本実施形態の位置決め制御装置は、Ｓ字形状の位置指令だけでなく、任意形状の位置指令を用いた場合でも、負荷選択の振動を抑制することができるようにしたものである。
本実施形態の位置決め制御装置は、図１に示されるように、任意形状位置指令発生部１４と、ＣＩＣ（Ｃａｓｃａｄｅｄ Ｉｎｔｅｇｒａｔｏｒ ａｎｄ Ｃｏｍｂ：並列カスケード積分くし形）フィルタ１６を有している。
このＣＩＣフィルタ１６の伝達関数Ｆ2（ｓ）は、下記の式（４）により表される。
【数６】
0004340846.tif 000007
ただし、ｍは２以上の自然数である。また、［Ｂ1、Ｂ2、・・・、Ｂm］および［Ｔ1、Ｔ2、・・・、Ｔm］はそれぞれ後述するように無定常偏差条件および振動抑制条件により定める係数ベクトルおよび時定数ベクトルである。
まず、無定常偏差条件について考える。
良く知られているように、フィードバック制御部３が積分要素を含んでいれば、フィードバック系の位置指令Ｘrfとモータ位置Ｘmの定常偏差は必ず０となる。そのため、ここではＣＩＣフィルタ１６の入力である元の位置指令ＸrとＣＩＣフィルタ１６通過後のフィードバック系の位置指令Ｘrfとの定常偏差が０となるような無定常偏差条件について考える。
任意形状位置指令部１４が生成した元の位置指令Ｘrを下記の式（５）で表すものとする。
【数７】
0004340846.tif 000008
ただし、ここでＸeは目標送り距離であり、ｔeは指令時間である。
上記の式（４）より、元の位置指令ＸrがＣＩＣフィルタ１６を通過した後のフィードバック系の位置指令Ｘrfは、下記の式（６）により示される。
【数８】
0004340846.tif 000009
式（５）を考慮し、Ｔd＝ｍａｘ｛Ｔ1、Ｔ2、・・・、Ｔm｝、ｕ＝ξ−Ｔiとすると、すべてのｉ∈［１，ｍ］に対して、下記の式（７）が成立する。
【数９】
0004340846.tif 000010
式（７）を式（６）に代入すると、下記の式（８）が得られる。
【数１０】
0004340846.tif 000011
上記の式（８）から、明らかにｔ＞ｔe＋Ｔd時においてＸr（ｔ）≡Ｘe（無定常偏差条件）を満たすための必要十分条件は、下記の式（９）および式（１０）が同時に成り立つことである。
【数１１】
0004340846.tif 000012
【数１２】
0004340846.tif 000013
次に、振動抑制条件について考える。
図１より、負荷機械の振動極は、下記の式（１１）により示される。
【数１３】
0004340846.tif 000014
負荷機械の振動を抑制するため、ＣＩＣフィルタ１６にこの振動極をキャンセルできる零点を持たせる必要がある。ずなわち、振動抑制条件は下記の式（１２）により与えられる。
【数１４】
0004340846.tif 000015
上記の式（１２）を整理すると、振動抑制条件は、下記の式（１３）、（１４）により与えられる。
【数１５】
0004340846.tif 000016
【数１６】
0004340846.tif 000017
ただし、
【数１７】
0004340846.tif 000018
である。
最後に、係数ベクトル［Ｂ1、Ｂ2、・・・、Ｂm］および時定数ベクトル［Ｔ1、Ｔ2、・・・、Ｔm］を定める。
ＣＩＣフィルタ１６の係数ベクトルと時定数ベクトルを定める際に、無定常偏差条件の式（９）と式（１０）および振動抑制条件の式（１３）と式（１４）、全部で４つの方程式を同時に満たすようにしなければならない。
ｍが２以上であれば、２つ以上の係数と２つ以上の時定数、合わせて４つ以上となるため、式（９）と式（１０）と式（１３）と式（１４）の連立方程式を解くと、上記の未定係数を定めることができる。ところが、式（１３）と式（１４）は時定数ベクトルの非線形方程式であるため、上記の連立方程式を解くことは大変困難である。
そのため、ｍを４以上に設定し、予め時定数ベクトル［Ｔ1、Ｔ2、・・・、Ｔm］を適当な値を与えておけば、式（９）と式（１０）と式（１３）と式（１４）は係数ベクトル［Ｂ1、Ｂ2、・・・、Ｂm］の線形連立方程式となるので、係数ベクトル［Ｂ1、Ｂ2、・・・、Ｂm］は簡単に求められる。
特に、ｍ＝３、Ｔ1＝０、Ｔ2＝π／ωd、Ｔ3＝２π／ωdとすれば、式（１４）は係数ベクトル［Ｂ1、Ｂ2、Ｂ3］とは関係なく、常に成り立つので、式（９）、式（１０）および式（１３）の連立方程式を解くと、下記の式（１５）が得られる。
【数１８】
0004340846.tif 000019
ただし、
【数１９】
0004340846.tif 000020
である。
このように、上記により求めたＣＩＣフィルタ１６を用いると、任意形状の元の位置指令Ｘrに対しても、制御系は定常偏差が残らず負荷先端１０の残留振動を抑制できる。
（第２の実施形態）
次に、本発明の第２の実施形態の位置決め制御装置について説明する。図２は本発明の第２の実施形態の位置決め制御装置の構成を示すブロック図である。図２において、図１中の構成要素と同一の構成要素には同一の符号を付し、説明を省略するものとする。
本実施形態の位置決め制御装置が、図１に示した第１の実施形態の位置決め制御装置と異なる構成は、図２では図１の連続系のＣＩＣフィルタ１６を離散化したことである。すなわち、離散化ＣＩＣフィルタ１７では、積分要素１／ｓがＴ／（１−ｚ-1）に変換され、遅れ要素ｅ-Tisがｚ-niに変換されている。ただし、ｚはｚ変換演算子、Ｔはサンプル周期、ｎiはＴi／Ｔの最も近い整数である。
以上のように離散化を行うと、プレフィルタの特性が少し変わるので、第２の実施形態で定めた係数ベクトル［Ｂ1、Ｂ2、・・・、Ｂm］をそのまま用いると、式（９）と式（１０）の無定常偏差条件および式（１３）と式（１４）の振動抑制条件は満たされなくなる可能性がある。一般に、振動抑制条件は少し外れても、近似的に満足されれば依然十分な振動抑制特性が得られるが、無定常偏差条件が僅かに外れて定常偏差が残ることは位置決め制御にとっては許されない。そのため、無定常偏差条件を厳密に満足するように係数ベクトル［Ｂ1、Ｂ2、・・・、Ｂm］を修正する必要がある。
以下、離散系の無定常偏差条件を与え、係数ベクトルを修正する方法を説明する。
まず、任意形状位置指令部１４が生成した元の位置指令Ｘrの離散形式を下記の式（１６）で表すものとする。
【数２０】
0004340846.tif 000021
ただし、Ｘeは目標送り距離、ｎeは指令のサンプル周期数である。
離散化ＣＩＣフィルタ１７は、下記の式（１７）のように表現される。
【数２１】
0004340846.tif 000022
そのため、Ｘrが離散化ＣＩＣフィルタ１７を通過した後のフィードバック系の位置指令Ｘrfは、下記の式（１８）のように示される。
【数２２】
0004340846.tif 000023
式（１６）を考慮し、ｎd＝ｍａｘ｛ｎ1，ｎ2，…，ｎm｝、ｌ＝ｋ−ｎi−ｊとすると、すべてのｉ∈［１，ｍ］に対して、下記の式（１９）が満たされる。
【数２３】
0004340846.tif 000024
式（１９）を式（１８）に代入すると、下記の式（２０）が得られる。
【数２４】
0004340846.tif 000025
上記の式（２０）から、明らかにｋ＞ｎe＋ｎd時においてＸr（ｋ）≡Ｘe（離散系の無定常偏差条件）を満たすための必要十分条件は式（１０）と、下記の式（２１）が同時に成り立つことである。
【数２５】
0004340846.tif 000026
すべてのｉ∈［１，ｍ］に対してＴiはＴの整数倍である場合は、下記の式（２２）が成り立つので、式（２１）は式（９）と同じになる。
【数２６】
0004340846.tif 000027
これは第１の実施の形態で定めた係数ベクトル［Ｂ1、Ｂ2、・・・、Ｂm］をそのまま用いても離散化系の無定常偏差条件も満たされることを意味する。
しかし、あるｉ∈［１，ｍ］に対してＴiはＴの整数倍でない場合は、式（２１）は式（９）と異なるので、第１の実施の形態で定めた係数ベクトル［Ｂ1、Ｂ2、・・・、Ｂm］をそのまま用いたら離散系の無定常偏差条件の式（２１）が満たされなくなる。そのため、係数ベクトル［Ｂ1、Ｂ2、・・・、Ｂm］の任意の２つ要素、例えば、Ｂ1とＢ2を修正係数とし、他の全ての要素を第１の実施の形態で定めた値に固定し、式（１０）と式（２１）で構成される連立方程式を解くことにより、新たに修正係数Ｂ1とＢ2の値を求める。このように最終に得た係数ベクトル［Ｂ1、Ｂ2、・・・、Ｂm］を用いると、制御系は定常偏差が残らない。
本実施形態の位置決め制御装置を用いた場合のシミュレーション結果を図３に示す。図３を参照すると、上記により求めた離散化ＣＩＣフィルタ１７を用いると、任意形状の元の位置指令Ｘrに対しても、制御系は定常偏差が残らず負荷先端１０の残留振動を抑制できることがわかる。
【０１３５】
【発明の効果】
以上説明したように、本発明によれば、負荷機械振動の減衰を考慮して位置決め完了後の残留振動を抑制し、定常偏差が残らないようにプレフィルタを構成し、位置指令発生部が生成した元の位置指令をプレフィルタに通させて得た信号をフィードバック制御系の位置指令とすることにより、負荷先端の残留振動を抑制し、高速かつ高精度位置決め制御ができるという効果を得ることができる。
【図面の簡単な説明】
【図１】本発明の第１の実施形態の位置決め制御装置の構成を示すブロック図である。
【図２】本発明の第２の実施形態の位置決め制御装置の構成を示すブロック図である。
【図３】本発明の第２の実施形態を用いた場合のシミュレーション結果を示す図である。
【図４】柔軟アームを有する機械の位置決め制御系の構成図である。
【図５】図４に示した従来の位置決め制御装置のシミュレーション結果を示す図である。
【図６】従来の他の位置決め制御装置の構成を示すブロック図である。
【図７】Ｓ字位置指令および、このＳ字位置指令の速度パターンおよび加速度パターンの時間関数を示す図である。
【図８】図６に示した従来の位置決め制御装置を用いた場合のシミュレーション結果を示す図である。
【符号の説明】
１ 位置指令発生部
２ プレフィルタ
３ フィードバック制御部
４ サーボドライバ
５ モータ
６ エンコーダ
７ トルク伝達機構
８ 負荷テーブル
９ 柔軟アーム
１０ 負荷先端
１１ フィードバック制御系の位置指令からモータ位置までの伝達関数
１２ モータ位置から負荷先端位置までの伝達関数
１３ Ｓ字位置指令発生部
１４ 任意形状位置指令発生部
１６ ＣＩＣフィルタ
１７ 離散化ＣＩＣフィルタ
１８ ２次フィルタ
Ｘr 元の位置指令
Ｖr Ｘrの微分値
Ａr Ｖrの微分値
Ｘrf フィードバック制御系の位置指令
Ｖrf Ｘrfの微分値
Ｘm モータの位置信号
Ｘh 負荷先端の位置
【図１】
0004340846.tif 000029
【図２】
0004340846.tif 000030
【図３】
0004340846.tif 000031
【図４】
0004340846.tif 000032
【図５】
0004340846.tif 000033
【図６】
0004340846.tif 000034
【図７】
0004340846.tif 000035
【図８】
0004340846.tif 000036