(57)【要約】      （修正有）【課題】加減速運動により運動体に生じる多自由度かつ時変の振動を抑制すること。【解決手段】参照入力値を振動モードごとの入力値に分解し、振動モードごとの入力値について、ｎ−ｋ次モードの振動を抑制するため入力位置が設定されたインパルスＡ０・・・Ａ２k−１からｎ−ｋ次モードの固有周期の半周期経過後に入力するようにインパルスＡ２k・・・Ａ２k+1−１の入力位置をそれぞれ設定し、加速または減速運度を行っている間に運動体自体の姿勢変化に伴って生じるコリオリの力がする仕事を考慮してインパルスＡ２k・・・Ａ２k+1−１の大きさをそれぞれ設定することを整数ｋ＝０からｎ−１まで順に実行することによって、入力位置と大きさがそれぞれ設定された合計２ｎ本のインパルスＡ０・・・Ａ２n−１を重ね合わせるインプットシェイピングにより入力値を整形し、整形された入力値から非線形項を考慮して関節ごとの加減速パターンを生成する。【選択図】図１７
2014052763.tif 000002
【特許請求の範囲】
【請求項１】
駆動装置による外力が付加されて、加速または減速運動が行われる運動体において、該加速または減速運動に伴って前記運動体に生じる、ｎ自由度かつ時変の振動を抑制する振動抑制方法において、
参照入力値を振動モードごとの入力値に分解するステップと、
前記振動モードごとの入力値について、（ｎ−ｋ）次モードの振動を抑制するため、入力位置が設定されたインパルスＡ０…Ａ２k−１から（ｎ−ｋ）次モードの固有周期の半周期経過後に入力するように、インパルスＡ２k…Ａ２k+1−１の入力位置をそれぞれ設定し、加速または減速運度を行っている間に前記運動体自体の姿勢変化に伴って生じるコリオリの力がする仕事を考慮して、前記インパルスＡ２k…Ａ２k+1−１の大きさをそれぞれ設定することを、整数ｋ＝０から（ｎ−１）まで順に実行することによって、入力位置と大きさがそれぞれ設定された合計２ｎ本のインパルスＡ０…Ａ２n−１を重ね合わせるインプットシェイピングにより入力値を整形するステップと、
整形された前記振動モードごとの入力値から、非線形項を考慮して関節ごとの加減速パターンを生成するステップと、
を有することを特徴とする振動抑制方法。
【請求項２】
駆動装置による外力が付加されて、加速または減速運動が行われる運動体において、該加速または減速運動に伴って前記運動体に生じる、ｎ自由度かつ時変の振動を抑制する振動抑制装置において、
参照入力値を振動モードごとの入力値に分解し、
前記振動モードごとの入力値について、（ｎ−ｋ）次モードの振動を抑制するため、入力位置が設定されたインパルスＡ０…Ａ２k−１から（ｎ−ｋ）次モードの固有周期の半周期経過後に入力するように、インパルスＡ２k…Ａ２k+1−１の入力位置をそれぞれ設定し、加速または減速運度を行っている間に前記運動体自体の姿勢変化に伴って生じるコリオリの力がする仕事を考慮して、前記インパルスＡ２k…Ａ２k+1−１の大きさをそれぞれ設定することを、整数ｋ＝０から（ｎ−１）まで順に実行することによって、入力位置と大きさがそれぞれ設定された合計２ｎ本のインパルスＡ０…Ａ２n−１を重ね合わせるインプットシェイピングにより入力値を整形し、
整形された前記振動モードごとの入力値から、非線形項を考慮して、関節ごとの加減速パターンを生成する
ことによって運動体の振動抑制を行う振動抑制装置。
【請求項３】
複数の駆動装置と該駆動装置を制御する制御装置とを備え、装置自体の姿勢変化をさせながら前記駆動装置によって加速または減速運動を行う際に生じる、ｎ自由度かつ時変の振動を抑制する振動抑制機能を備えた産業用機械装置において、
前記制御装置は、
参照入力値を振動モードごとの入力値に分解し、
前記振動モードごとの入力値について、（ｎ−ｋ）次モードの振動を抑制するため、入力位置が設定されたインパルスＡ０…Ａ２k−１から（ｎ−ｋ）次モードの固有周期の半周期経過後に入力するように、インパルスＡ２k…Ａ２k+1−１の入力位置をそれぞれ設定し、加速または減速運度を行っている間に装置自体の姿勢変化に伴って生じるコリオリの力がする仕事を考慮して、前記インパルスＡ２k…Ａ２k+1−１の大きさをそれぞれ設定することを、整数ｋ＝０から（ｎ−１）まで順に実行することによって、入力位置と大きさがそれぞれ設定された合計２ｎ本のインパルスＡ０…Ａ２n−１を重ね合わせるインプットシェイピングにより入力値を整形し、
整形された前記振動モードごとの入力値から、非線形項を考慮して、関節ごとの加減速パターンを生成する加減速パターン演算部を備え、
前記加減速パターン演算部により生成された前記加減速パターンに基づいて前記駆動装置を制御することにより、ｎ自由度かつ時変の振動を抑制する
ことを特徴とする産業用機械装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、加速または減速運動に伴って運動体に生じる振動を抑制する振動抑制方法および装置、特に、上記運動中に運動体自体の姿勢が変化する場合に、運動体に生じる、多自由度かつ時変の振動を抑制する振動抑制方法および装置に関する。
【背景技術】
【０００２】
様々な作業を行うために運動動作を行うロボットアームは、モータとアームの間に減速機を用いている。減速機はねじりばねとして振る舞うため、モータ停止後もアームの振動が残留する。その結果、振動が収まるまで次の作業に移ることができず、作業効率を向上させることができない。
【０００３】
このような残留振動に伴う課題は、ロボットアームだけに限られず、加速または減速運動を含む運動動作を行う運動体、例えば、工作機械、クレーン、油圧ショベル等についても共通する。
【０００４】
フィードバック制御で振動を抑える場合は検出器が必要となるため、検出器が不要なフィードフォワード制御で振動を抑える方法が求められている。
【０００５】
フィードフォワード制御での振動抑制方法としては、インプットシェイピング法が知られている（例えば、特許文献１、特許文献２、非特許文献１、非特許文献２）。このインプットシェイピング法は、工作機械などのように、固有振動数が変化しない場合に有効な振動抑制方法であり、様々な提案がなされている。また、制御指令から固有振動を引きおこす周波数生分を取り除くため、ノッチフィルタを用いる方法や、適応的にカルマンフィルタを用いる方法が知られている（特許文献３）。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−０２９６１７号公報
【特許文献２】特開２０１１−１４８０７８号公報
【特許文献３】特開２００９−０４２１７９号公報
【非特許文献】
【０００７】
【非特許文献１】Neil C. Singer and Warren P. Seering : Preshaping Command Inputs to Reduce System Vibration, Trans.ASME Journal of Dynamic Systems, Measurement, and Control, Vol.112、 No.1, 76-82 (1990)
【非特許文献２】Pyung Hun Chang and Hyung-Soon Park : Time-varying input shaping technique applied to vibration reduction of an industrial robot, Control Engineering Practice 13 (2005) 121-130
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ロボットアームは、様々な作業に対応可能とするために、一般に多関節および多自由度を有している。そのため、ロボットアームにおいて、一の関節が駆動された運動動作を行っている間に他の関節が駆動されると、運動動作を行っているアーム自体の姿勢が変化する。そのため、アームの固有振動数が時間によって変化することになる。このような場合にあっては時変の振動系となるため、時不変の振動系を対象として従来のインプットシェイピング法を単に適用するだけでは、振動抑制に対応することができない。
【０００９】
このような時変の振動系において振動を抑制する方法としては、いくつかの提案がなされている（例えば、特許文献１、特許文献２および非特許文献２参照）。しかしながら、例えば特許文献２は、１自由度かつ時変の振動抑制に関するものであり、多自由度かつ時変の振動抑制に対してインプットシェイピング法を普遍的に適用できる振動抑制方法が求められている。
【００１０】
従って、本発明の目的は、上記課題を解決することにあって、加速または減速運動に伴って運動体に生じる、多自由度かつ時変の振動を、インプットシェイピング法を普遍的に適用して抑制する振動抑制方法を提供することにある。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、本願発明に係る振動抑制方法は、駆動装置による外力が付加されて、加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じる、ｎ自由度かつ時変の振動を抑制する振動抑制方法において、参照入力値を振動モードごとの入力値に分解するステップと、前記振動モードごとの入力値について、（ｎ−ｋ）次モードの振動を抑制するため、入力位置が設定されたインパルスＡ０・・・Ａ２k−１から（ｎ−ｋ）次モードの固有周期の半周期経過後に入力するように、インパルスＡ２k・・・Ａ２k+1−１の入力位置をそれぞれ設定し、加速または減速運度を行っている間に前記運動体自体の姿勢変化に伴って生じるコリオリの力がする仕事を考慮して、前記インパルスＡ２k・・・Ａ２k+1−１の大きさをそれぞれ設定することを、整数ｋ＝０から（ｎ−１）まで順に実行することによって、入力位置と大きさがそれぞれ設定された合計２ｎ本のインパルスＡ０・・・Ａ２n−１を重ね合わせるインプットシェイピングにより入力値を整形するステップと、整形された前記振動モードごとの入力値から、非線形項を考慮して、関節ごとの加減速パターンを生成するステップと、を有することを特徴とするものである。
【発明の効果】
【００１２】
本発明によれば、加速または減速運動に伴って運動体に生じる、ｎ自由度（ｎ≧２）かつ時変の振動をインプットシェイピング法を用いて抑制する場合に、（ｎ−ｋ）次モードの振動を抑制するため、入力位置が設定されたインパルスＡ０・・・Ａ２k−１から（ｎ−ｋ）次モードの固有周期の半周期経過後に入力するように、インパルスＡ２k・・・Ａ２k+1−１の入力位置をそれぞれ設定し、加速または減速運度を行っている間に運動体自体の姿勢変化に伴って生じるコリオリの力がする仕事を考慮して、前記インパルスＡ２k・・・Ａ２k+1−１の大きさをそれぞれ設定することを、整数ｋ＝０から（ｎ−１）まで順に実行することによって、入力位置と大きさがそれぞれ設定された合計２ｎ本のインパルスＡ０・・・Ａ２n−１を重ね合わせることで、効果的に振動を抑制することができる。
【図面の簡単な説明】
【００１３】
【図１】本発明の実施の形態１に係る振動抑制方法が適用されるロボットアームの模式図である。
【図２】図１のロボットアームの動的モデルを示す図である。
【図３】インプットシェイピング法が適用される１自由度系の動的モデルを示す図である。
【図４】１自由度時不変系における２本のインパルス応答の変位と時間の関係を示す図である。
【図５】２本のインパルス入力を示す図である。
【図６】２本のインパルス入力の周波数応答を示す図である。
【図７】３本のインパルス入力を示す図である。
【図８】３本のインパルス入力の周波数応答を示す図である。
【図９】４本のインパルス入力を示す図である。
【図１０】４本のインパルス入力の周波数応答を示す図である。
【図１１】２自由度時変系における物理座標系での２本のインパルス応答の変位と時間の関係を示す図である。
【図１２】モード座標系での２本のインパルス応答の変位と時間の関係を示す図である。
【図１３】モード座標系での１次モードのインパルス応答の変位と時間の関係を示す図である。
【図１４】モード座標系での２次モードのインパルス応答の変位と時間の関係を示す図である。
【図１５】モード座標系での２次モードのインパルス応答の変位と時間の関係を示す図である。
【図１６】モード座標系での１次モードのインパルス応答の速度と時間の関係を示す図である。
【図１７】モード座標系での１次モードのインパルス応答の変位と時間の関係を示す図である。
【図１８】モード座標系での２次モードのインパルス応答の変位と時間の関係を示す図である。
【図１９】物理座標系での１次モードのインパルス応答の変位と時間の関係を示す図である。
【図２０】物理座標系での２次モードのインパルス応答の変位と時間の関係を示す図である。
【図２１】ロボットコントローラの構成を示すブロック図である。
【図２２】振動抑制方法を適用して駆動装置により運動体の運転を行う手順のフローチャートである。
【図２３】アームの動作の全継続時間における本発明による整形前の加速度パターンを示す図である。
【図２４】アームの動作の全継続時間における本発明による整形後の加速度パターンを示す図である。
【図２５】１、２、３およびｎ自由度系におけるインパルス入力の本数と間隔を示す図である。
【００１４】
実施の形態１．
本発明の実施の形態１に係る運動体の振動抑制方法について、運動体の一例であるロボットアーム１０を用いて説明する。また、このロボットアームの模式図を図１に示す。
【００１５】
図１に示すように、ロボットアーム１０は、複数のアームと、それぞれのアームを回転運動可能に連結する複数の関節と、先端のアームに取り付けられたハンドツールとを備えている。具体的には、ロボットアーム１０には、関節１１ａ、１１ｂ、１１ｃと、アーム１２ａ、１２ｂと、アーム１２ｂの先端に取り付けられたハンドツール１３とが備えられている。さらに、それぞれの関節１１ａ〜１１ｃにおいて、アーム１２ａ、１２ｂを回転駆動させる駆動装置（例えば、モータ等、図示せず）と、この駆動装置からの駆動力をそれぞれの関節に伝達させる減速機（図示せず）とが備えられている。
【００１６】
これら駆動装置は、ロボットコントローラ２０によって制御される。ロボットコントローラ２０は、予め記憶しているロボットプログラム、または、インターフェイス３０を介して入力された作業者からの命令に従って、ロボットアーム１０の動作を制御する。なお、ロボットコントローラ２０の構成については、後に詳細に説明する。
【００１７】
図１のロボットアーム１０において、関節１１ａは、鉛直方向の回転軸１４ａ周りにアーム１２ａを旋回（旋回角度Θ）させることができ、関節１１ｂは、水平方向の回転軸１４ｂ周りにアーム１２ａを回転運動（回転角度Ψ１）させることができ、関節１１ｃは、水平方向の回転軸１４ｃ周りにアーム１２ｂを回転運動（回転角度Ψ２）させることができる。そのため、ロボットアーム１０の動きは、垂直方向の回転軸１４ａ周りの旋回運動（以降、「旋回方向の動き」とする。）と、水平方向の回転軸１４ｂおよび１４ｃ周りの回転運動（以降、「面内（垂直面内）方向の動き」とする。）に分けることができる。
【００１８】
ロボットアーム１０において、旋回方向の動きと面内方向の動きとが同時に行われて、加速または減速運動が行われると、その後、旋回方向と面内方向に自由振動が生じることになる。なお、特許文献２では、旋回方向の１自由度系の振動について取り扱ったが、本実施の形態１では、面内方向の２自由度系の振動について取り扱うものとする。
【００１９】
このような２自由度系の振動を抑制する、本実施の形態１の振動抑制方法を説明するにあたって、図１のロボットアームを図２のように面内方向にモデル化した際の運動方程式について、まず説明する。
【００２０】
パラメータとしてアームの慣性モーメントをＪ１（ｔ）、Ｊ２（ｔ）、減速機の回転ばね定数をｋ１、ｋ２、モータの指令回転角度をφ０１、φ０２、アームの実回転角度をφ１、φ２とする。ただし、（ｔ）は時間の関数を示す。また、今回は減速機等で発生する粘性減衰を無視する。角運動量の変化は外力のモーメントに等しいので、次式（１）、(２）のように表すことができる。
【００２１】
【数１】
2014052763.tif 000003
【００２２】
【数２】
2014052763.tif 000004
式（１）、(２）を展開すると次の運動方程式（３）、（４）が導かれる。なお、ここで・は時間による微分を示す。
【００２３】
【数３】
2014052763.tif 000005
【００２４】
【数４】
2014052763.tif 000006
ここで、式（１）、(２）の相対角変位を式（５）（６）とおいて式（３）、（４）に代入して整理すると、次式（７）、(８）のように表すことができる。
【００２５】
【数５】
2014052763.tif 000007
【００２６】
【数６】
2014052763.tif 000008
【００２７】
【数７】
2014052763.tif 000009
【００２８】
【数８】
2014052763.tif 000010
【００２９】
式（７）、（８）の左辺第１項はコリオリの力である。また、右辺は外力であるため、モータによる強制変位の振動系となる。一般にモータは台形速度則に沿って運転される。台形則を前提にすると外力は既知関数になり、相対角変位はインパルス応答と外力の畳み込み積分で求まる。
【００３０】
ここで、工作機械など、運動方程式の係数が変化しない時不変系で一般的に用いられており、系の固有振動数から振動を打ち消しあう入力を定める方法にインプットシェイピング法がある。
【００３１】
例えば、図３のような外力ｆ、変位ｘ、質量ｍ、ばねｋのみの１自由度時不変系の場合、運動方程式は次式となる。
【００３２】
【数９】
2014052763.tif 000011
【００３３】
外力として時刻ｔ０に、大きさＡのインパルスを入力し、系の半周期後の時刻ｔ１に同じ大きさＡのインパルスを入力した際の応答を図４に示す。２つの応答を足し合わせた応答は時刻ｔ１以降ではゼロとなる。すなわち、系の固有周期のちょうど半周期の位置に１対のインパルスを発生させると、半周期以降の振動がゼロとなる。
【００３４】
また、ｋΔＴ間隔の２本のインパルス入力の周波数応答を図５、６に示す。ここで、ｎ、ｋは自然数、ΔＴはサンプリング周期とする。図６の周波数領域でみると、谷に位置する周波数は２本のインパルス間隔の０．５周期、１．５周期となるノッチフィルタとなるため、同じ大きさの２つのインパルス入力は特定周波数の振動を励起しないと言える。なお、図５、６のグラフは、次式（１０）、（１１）に基づくものである。
【００３５】
【数１０】
2014052763.tif 000012
【００３６】
【数１１】
2014052763.tif 000013
【００３７】
次に、２自由度時不変系では固有振動数が２つ存在する。そのため、インパルスをもう１本入力して３本のインパルスを入力した場合に、ノッチフィルタのノッチ周波数を検討した。
【００３８】
式（１３）に３本のインパルスのフーリエ級数展開を、図７、８にその計算例（インパルス入力の大きさα＝β＝１、位置ｋ＝２、ｌ＝９を示す。ここで、ｎ、ｋ、ｌは自然数（ｌ＞ｋ）、ΔＴはサンプリング周期とする。３本のインパルスではノッチ周波数を任意の位置に設計することは困難だった。
【００３９】
【数１２】
2014052763.tif 000014
【００４０】
【数１３】
2014052763.tif 000015
【００４１】
１自由度系ではインパルス２本で半周期以降の振動が抑制されていることから、２次の固有振動数を先に抑制することを考え、２本のインパルス間隔を２次の固有周期の半分に設定する。次に、１次の固有振動数を抑制するため、この２本を１セットとし１次の固有周期の半分の間隔で系にもう１セット加える。入力するインパルスは計４本となる。この４本のインパルスのフーリエ変換を図９、１０に示す。ここで、ｎ、ｋ、ｌ、ｍは自然数（ｍ＞ｌ、ｋ＝ｍ−ｌ）、ΔＴはサンプリング周期とする。
【００４２】
図１０において、谷に位置する周波数は１本目と２本目のインパルス間隔の半周期ω＝π／（ｋΔＴ）、１本目と３本目のインパルス間隔の半周期ω＝π／（ｌΔＴ）である。これより、１次、２次に対応したノッチフィルタを設計できたことになる。そのため、２自由度時変系では系に合計４本インパルスを加えることで振動を抑制する。なお、図９、１０のグラフは、次式（１４）から（１６）に基づくものである。
【００４３】
【数１４】
2014052763.tif 000016
【００４４】
【数１５】
2014052763.tif 000017
【００４５】
【数１６】
2014052763.tif 000018
【００４６】
以上、２自由度時不変系のインパルス応答について説明した。次に、当初のロボットアームのモデルに戻って、２自由度時変系のインパルス応答について説明する。
【００４７】
時変系では慣性モーメントが時間変化する。式（７）、（８）の右辺をそれぞれｆ１、ｆ２とし、式（７）の右辺ｆ１の時刻ｔ０に大きさＡ０のインパルスを入力する。デルタ関数δ（ｔ）を用いると、ｆ１＝Ａ０δ（ｔ−ｔ０）となる。これは、モータ１のみ動かすことに相当する。これにより、次式（１７）、（１８）が導かれる。
【００４８】
【数１７】
2014052763.tif 000019
【００４９】
【数１８】
2014052763.tif 000020
【００５０】
図１１に示すように、時変系では慣性モーメントが時間変化するため固有振動数が時間変化し、また、式（１７）、（１８）には連成項が存在するため振動を抑制するインパルス入力の位置が定まらない。そこで、モード座標系に変換し、時変系に対応したインパルスの入力方法を検討する。
【００５１】
慣性モーメントが時間変化するため、固有ベクトルが時間変化することになる。１次、２次の固有振動数に対する式（１９）を固有ベクトルＲ１（ｔ）、Ｒ２（ｔ）、モード座標系での変位を［ｑｒ（ｔ）］、（ｒ＝１、２）とすると、式（２０）が導かれる。
【００５２】
【数１９】
2014052763.tif 000021
【００５３】
【数２０】
2014052763.tif 000022
【００５４】
式（１７）、（１８）の両辺に式（２０）を代入し、直交性を利用することでモード座標系でのパラメータが求まる。時刻ｔ０に大きさＡ０のインパルスを入力した式（１７）、（１８）のモード座標系での運動方程式は、次式（２１）、（２２）のようになる。
【００５５】
【数２１】
2014052763.tif 000023
【００５６】
【数２２】
2014052763.tif 000024
【００５７】
ただし、´（ダッシュ)はモード座標系でのパラメータであることを示し、モード質量を式（２３）、モード剛性を式（２４）とする。固有ベクトルが時間変化するため、モード座標系ではモード剛性も時間変化する。
【００５８】
【数２３】
2014052763.tif 000025
【００５９】
【数２４】
2014052763.tif 000026
【００６０】
モード座標系でのインパルス応答は、図１２のようになる。固有振動数が変化し、コリオリの力が仕事をするため振幅も変化するが、モード座標系で重ね合わせの原理を用い、振動を抑制するインパルス入力の位置・大きさを以下に検討する。
【００６１】
振幅、固有振動数の変化に対応した４本のインパルス入力を求める。４本のインパルスは２本を１セットとしており、１セットの時間間隔は２次モードが打ち消される間隔であるため入力位置が先に求まる。そのため、入力の位置を決めた後入力の大きさを求める。入力の大きさをＡｉ、位置をｔｉ（ｉ＝１、２・・・）とする。
【００６２】
図１３、１４は４本のインパルス応答であり、変位がゼロになる時刻を半周期とする。図１４の２次モードの応答より、２本目の入力位置は１本目の半周期後、同様に４本目の入力位置は３本目の半周期後である。３本目の位置は図１３の１次モードの応答より１本目と２本目の合成波ｑ１（ｔ０＋ｔ１）を３本目と４本目の合成波ｑ１（ｔ２＋ｔ３）で打ち消すことができる位置となる。
【００６３】
次に入力の大きさを求める。４本のインパルスは２本を１セットとし２次モードを抑制するため、２本目Ａ１と４本目Ａ３の大きさは２次モードから求め、最初の１セットと最後の１セットで１次モードを抑制するため３本目Ａ２の大きさは１次モードから求める。振幅の変化を考慮するため式（２２）の両辺にｄｑ２（ｔ）／ｄｔを掛け時間で積分することでエネルギの関係式（２５）が得られる。つまり、半周期の間にコリオリの力がする仕事と固有ベクトルが時間変化する分を考慮した大きさを加えればよい。まず、大きさＡ１について述べる。
【００６４】
【数２５】
2014052763.tif 000027
【００６５】
式（２５）右辺の第３項Ａ０ｄｑ２／ｄｔは、系に加えられたエネルギで時間変化はないが、インパルスを加える時刻によって異なるため式（２６）とする。右辺の第１項はコリオリの力、第２項は固有ベクトルの変化による仕事であり時間により変化するため、式（２７）とおいて整理すると、式（２８）が導かれる。
【００６６】
【数２６】
2014052763.tif 000028
【００６７】
【数２７】
2014052763.tif 000029
【００６８】
【数２８】
2014052763.tif 000030
【００６９】
式（２８）より運動エネルギとポテンシャルエネルギに相当する項が時間変化することが分かる。式（２８）において、変位がゼロとなる時刻では左辺第２項は０となり、時刻ｔ０と時刻ｔ１では以下の式（２９）から（３１）が求まる。
【００７０】
【数２９】
2014052763.tif 000031
【００７１】
【数３０】
2014052763.tif 000032
【００７２】
【数３１】
2014052763.tif 000033
【００７３】
インパルス入力は系の変位を変えず初期速度を与えることと等しい。そのため、時刻ｔ１において式（３２）となればよい。そこで、Ａ１＝α１Ａ０とおき式（２９）〜（３１）から２次モードを打ち消す入力の割合α１が式（３３）から求まる。
【００７４】
【数３２】
2014052763.tif 000034
【００７５】
【数３３】
2014052763.tif 000035
【００７６】
図１５より、コリオリの力、固有ベクトルの変化による仕事を考慮した大きさのインパルスを入力することで時刻ｔ１以降の振動が抑制されていることが分かる。
【００７７】
同様に、大きさＡ３の値を求める。大きさＡ２の値は未知であるが、入力の割合は入力する時刻に依存し、大きさＡ２の値には依存しない。入力する時刻が既知であるため、式（２８）において、時刻ｔ２と時刻ｔ３で式（２９）〜（３１）の関係が成り立つのでＡ３＝α２Ａ２となる入力の割合α２が次式（３４）のように求まる。つまり、大きさＡ２が求まれば、大きさＡ３は決まる。
【００７８】
【数３４】
2014052763.tif 000036
【００７９】
次に、大きさＡ２を求める。式（２１）にｄｑ１（ｔ）／ｄｔを掛け時間で積分すると１次モードのエネルギの次式が得られる。
【００８０】
【数３５】
2014052763.tif 000037
【００８１】
Ａ０ｄｑ１／ｄｔは系に加えられたエネルギで時間変化はないが、インパルスを加える時刻によって異なるため式（３６）とする。右辺の第１項はコリオリの力、第２項は固有ベクトルの変化による仕事であり時間により変化するため、式（３７）とおいて、式（３５）を整理すると、式（３８）が導かれる。
【００８２】
【数３６】
2014052763.tif 000038
【００８３】
【数３７】
2014052763.tif 000039
【００８４】
【数３８】
2014052763.tif 000040
【００８５】
式（３８）より運動エネルギとポテンシャルエネルギに相当する項が時間変化することが分かる。また、図１６はインパルスを４本加えた際の１次モードの速度に対するインパルス応答である。Ａ３＝α２Ａ２より大きさＡ２は、時刻ｔ３での系の速度と入力する初期速度が等しくなるようにすればよい。振幅の変化を考慮し、時刻ｔ３でのエネルギ保存の関係から次式が成り立ち、これにより大きさＡ２が求まる。
【００８６】
【数３９】
2014052763.tif 000041
【００８７】
インパルスは初期速度を系に与えることであり、重ね合わせの原理より、各項のエネルギの値は以下の式（４０）から（４６）のようになる。ただし、ｄｑ１ｔ０（ｔ０）／ｄｔのｄｑ１ｔ０／ｄｔは時刻ｔ０にインパルスを与えた際のインパルス応答、（ｔ０）は時刻ｔ０の速度とする。
【００８８】
【数４０】
2014052763.tif 000042
【００８９】
【数４１】
2014052763.tif 000043
【００９０】
【数４２】
2014052763.tif 000044
【００９１】
【数４３】
2014052763.tif 000045
【００９２】
【数４４】
2014052763.tif 000046
【００９３】
【数４５】
2014052763.tif 000047
【００９４】
【数４６】
2014052763.tif 000048
【００９５】
以上より、振動を抑制するインパルス入力４本すべての大きさが理論的に求まる。モード座標系で４本入力した際のインパル応答を図１７、１８に示す。図１７、１８において、各時刻に入力した際のインパルス応答を破線、一点鎖線および二点鎖線で示す。
【００９６】
図１７の１次モードの応答波形において、時刻ｔ０と時刻ｔ１の応答を足し合わせた応答ｑ１（ｔ０＋ｔ１）を細い実線、時刻ｔ２と時刻ｔ３の応答を足し合わせた応答ｑ１（ｔ２＋ｔ３）を実線、時刻ｔ０から時刻ｔ３の応答を全て足し合わせた応答ｑ１（ｔ０＋ｔ１＋ｔ２＋ｔ３）を太い実線で示す。最初の１セットと最後の１セットで１次モードが抑制されている。
【００９７】
また、図１８の２次モードの応答波形において、１セットの１本目と２本目で２次モードが抑制されている。そのため、時刻ｔ３以降の振動が抑制されていることがわかる。
【００９８】
また、モード座標系に入力した４本のインパルス入力の位置・大きさを変えずに物理座標系に４本のインパルスを入力した際のインパルス応答を図１９、２０に示す。図１９、２０において、各時刻に入力した際のインパルス応答を破線、一点鎖線および二点鎖線で示す。
【００９９】
図１９、２０において、時刻ｔ０と時刻ｔ１の応答を足し合わせた応答を細い実線、時刻ｔ２と時刻ｔ３の応答を足し合わせた応答を実線、時刻ｔ０から時刻ｔ３の応答を全て足し合わせた応答を太い実線で示す。時刻ｔ０から時刻ｔ３の応答を全て足し合わせた応答からわかるように、時刻ｔ３以降の振動が抑制されていることから、２自由度時変系の自由振動は本手法で振動抑制が可能であると言える。
【０１００】
また、時刻ｔ３以降の振動が抑制されているが、これは、時変系でも基本的に２次モードの振動は１次モードの振動に比べて固有周期が短いため、１次モードの半周期分にわずかな時間を追加するだけで、２次モードの振動まで抑制できることを示している。
【０１０１】
以上により、ロボットアームを２自由度系でモデル化し、ロボットアームの姿勢が変化するため、固有振動数が変化する時変系の振動抑制方法を提案した。１自由度系では２本のインパルスを入力することで振動を抑制されているが、２自由度系では２本のインパルスを１セットと考え、この１セットでは２次モードを励起しない間隔にインパルスを配置した。最初の１セットは２次モードを励起しないが１次モードを励起する。そこで、最初の１セットが励起した１次モードを打ち消すように２番目の１セットを配置した。結局、４本のインパルスを配置しコリオリの力とモードの変化を考慮してエネルギのつりあいを考えて、各インパルスの振幅を決定した。これより、理論的には２自由度時変系の自由振動を抑制可能となった。
【０１０２】
次に、上述の振動抑制方法を用いて駆動装置を制御するためのロボットコントローラ２０の構成について、図２１を参照らしながら説明する。
【０１０３】
ロボットコントローラ２０は、ロボットプログラムの実行やロボットの指令値の計算を行う演算部（ＣＰＵ）２０１、ロボットプログラムや演算結果を記憶しておく記憶装置（ＲＯＭ、ＲＡＭおよびハードディスク等）２０２、インターフェイス３０等の外部の機器と通信する入力出装置２０３、および、演算部２０１で計算した指令値を元に駆動装置を駆動制御するサーボ制御部２０４を備えるハードウェア構成とすることができる。
【０１０４】
演算部２０１には、上述の振動抑制方法を実現するため、加減速パターン演算部２０１ａを備えている。当該加減速パターン演算部２０１ａによって、ロボットアーム１０に振動を抑制した運転を行わせるための駆動装置の加減速パターンを計算（シェイピング）する。
【０１０５】
次に、ロボットコントローラ２０によって、ロボットアーム１０に振動抑制した運転を行わせる手順について、図２２のフローチャートを参照しながら詳細に説明する。
【０１０６】
ステップＳ１において、駆動装置を用いて運転される運動体の物理パラメータの同定を行う。すなわち、まず、運動体の姿勢を変えて自由度の数だけ固有振動数を計測する。次に、減衰を考慮しない運動体方程式から実固有値解析により求まる固有振動数が、実際に計測された固有振動数に対してよい近似となるように、最小２乗法により各関節のねじりばね定数を同定する。
【０１０７】
ステップＳ２において、運動体の運転の加減速パターンを設定する。すなわち、まず、設計値から得られるパラメータとステップＳ１で同定したパラメータを入力する。次に、参照入力値（参照加減速パターン）を関節ごとに決めて入力する。
【０１０８】
ステップＳ３において、ステップＳ２で設定された運動体の加減速パターンに基づいて、残留振動を抑制するような駆動装置による運動体の加速度パターンのシェイピングを行う。この加速度パターンのシェイピングは、まず、ステップＳ２で入力した参照入力値を振動モードごとの入力に分解する。次に、既に説明した振動抑制方法、すなわち振動モードごとに、固有周期の変化に対する補正も考慮したインプットシェイピングにより、入力値整形する。最後に、整形されたモードごとの入力値から、非線形項を考慮して、関節ごとの加減速パターンを生成する。また、この加速度パターンのシェイピングは、特に自由振動が問題となる可能性が高い減速区間において行うことが望ましい。
【０１０９】
ステップＳ４において、ステップＳ３でシェイピングされた加速度パターンを２回積分した関節ごとの指令値を運動体に対して指令し、駆動装置を駆動して、運動体の加速、定速、減速運転を実施する。これによって、運動体において、残留振動が抑制された運転を実施することができる。
【０１１０】
以上のフローチャートによれば、ロボットアーム１０に振動を抑制した運動を行わせるための駆動装置の加減速パターンを計算することができる。なお、図２３に、関節ごとに入力を行う参照入力値（整形前参照加減速パターン）の１例を示す。また、図２４に、図２３の参照入力値を、上述の方法によってシェイピングした結果の加減速パターン（整形後参照加減速パターン）を示す。このシェイピング前後の加減速パターンを見比べてわかるように、加速を開始してから減速が終了するまでの総時間は、シェイピングを行ってもあまり長くはならない。
【０１１１】
なお、ステップＳ１〜Ｓ４までのパラメータの同定や加減速パターンのシェイピング等の計算処理は、例えば、ロボットコントローラ２０において実施することができる。また、このような場合に代えて、上述の計算処理を行うプログラムがインストールされたコンピュータ装置を用いて、ステップＳ１〜Ｓ３のパラメータの同定や計算処理を行い、シェイピングされた加速度パターンの情報を、ロボットコントローラ２０に入力して、駆動装置の駆動動作がロボットコントローラ２０により制御されても良い。
【０１１２】
また、運動体が複数の運転パターンを有するような場合にあっては、個々の運転パターン毎に、残留振動を抑制するような駆動装置による加速度パターンを計算して、制御装置に入力しておき、選択された運転パターンに対応する加速度パターンにて駆動装置の駆動動作を制御することもできる。この運転パターンの選択の際には、適切な運転パターンを選択する手段（例えば、センサなどを用いた手段など）を用いることもできる。
【０１１３】
なお、選定できる加速度の大きさは、駆動装置の能力（モータの能力など）により制限があるため、加速度パターンを相似変形させる限度は、駆動装置の能力の範囲内とする必要がある。そのため、駆動装置の能力を超えるような場合には、一対の区間の数を増大、すなわち動作の継続時間を増大させて、速度パターン、加速度パターンを再度算出する。
【０１１４】
また、時変系では、運動体が一定速度で運動している場合でも加振力が生じるため、一対の隣接区間の前半区間において加速度をゼロと設定し、後半区間で前半区間の振動を打ち消すように加減速値を決めても良い。
【０１１５】
実施の形態２．
実施の形態１では２自由度時変系の振動抑制方法について説明したが、次に、実施の形態２として３自由度時変系の場合について、図２５を参照しながら、以下に説明する。図２５は、１、２、３およびｎ自由度系におけるインパルス入力の本数とそれらの間隔を示す図である。
【０１１６】
３自由度系の場合も、振動を抑制するのに必要なインパルスの入力本数は、２自由度系の場合と同様に考えることができる。すなわち、２自由度系ではインパルス４本で半周期以降の振動が抑制されていることから、３自由度系の場合、３次モードの振動を先に抑制することを考え、２本のインパルスＡ０、Ａ１の間隔（すなわち、ｔ０からｔ１間）を３次モードの半周期にする。次に、２次モードの振動を抑制するため、この２本を１セットとし２次モードの半周期の間隔で系にもう１セットＡ２、Ａ３加える。さらに、１次モードの振動を抑制するため、これら４本を１セットとし１次モードの半周期の間隔で系にもう１セットＡ４、Ａ５、Ａ６、Ａ７加える。
【０１１７】
したがって、入力するインパルスは、Ａ０からＡ７の２ｎ＝２２＝計８本となり、これら全てのインパルスの入力位置が先に決まった。
【０１１８】
ここで、２自由度系におけるエネルギ保存式（２８）、（３８）と同様に、３自由度系における１次モードから３次モードのエネルギ保存式を求めると、次式(４７）から（４９）となる。なお、ここでは詳細な計算は省力する。
【０１１９】
【数４７】
2014052763.tif 000049
【０１２０】
【数４８】
2014052763.tif 000050
【０１２１】
【数４９】
2014052763.tif 000051
【０１２２】
これらエネルギ保存式(４７）から（４９）より、Ａ１＝α１Ａ０、Ａ３＝α２Ａ２、Ａ５＝α３Ａ４、Ａ７＝α４Ａ６となる入力の割合α１、α２、α３、α４はそれぞれ、次式（５０）から（５３）のように求まる。
【０１２３】
【数５０】
2014052763.tif 000052
【０１２４】
【数５１】
2014052763.tif 000053
【０１２５】
【数５２】
2014052763.tif 000054
【０１２６】
【数５３】
2014052763.tif 000055
【０１２７】
次に、２次モードのエネルギ保存式（４８）より収束計算を行うと、Ａ２およびＡ６の大きさについて次式（５４）、（５５）がそれぞれ成り立つ。
【０１２８】
【数５４】
2014052763.tif 000056
【０１２９】
【数５５】
2014052763.tif 000057
【０１３０】
続けて、１次モードのエネルギ保存式(４７）より収束計算を行うと、Ａ４の大きさについて次式が成り立つ。
【０１３１】
【数５６】
2014052763.tif 000058
【０１３２】
以上により、振動を抑制するインパルス入力８本すべての大きさが理論的に求まる。したがって、３自由度時変系の自由振動を抑制可能となった。
【０１３３】
実施の形態３．
次に、実施の形態３としてｎ自由度時変系（ｎは自然数)の場合について、図２５を参照しながら、以下に説明する。
【０１３４】
ｎ自由度系の場合も、２、３自由度系の場合と同様に考えることができる。すなわち、
ｎ自由度系の場合、ｎ次モードの振動を先に抑制することを考え、２本のインパルスＡ０、Ａ１の間隔（すなわち、ｔ０からｔ１間）をｎ次モードの半周期にする。次に、（ｎ−１）次モードの振動を抑制するため、この２本を１セットとし（ｎ−１）次モードの半周期の間隔（すなわち、ｔ０からｔ２間）で系にもう１セットＡ２、Ａ３加える。さらに、（ｎ−２）次モードの振動を抑制するため、これら４本を１セットとし（ｎ−２）次モードの半周期の間隔（すなわち、ｔ０からｔ４間）で系にもう１セットＡ４、Ａ５、Ａ６、Ａ７加える。同様に、（ｎ−ｋ）次モードの振動を抑制するため、既定のインパルス入力Ａ０・・・Ａ２k−１を１セットとし（ｎ−ｋ）次モードの半周期の間隔で系にもう１セットインパルス入力Ａ２k・・・Ａ２k+1−１を加えることを、整数ｋ＝３から（ｎ−１）まで順に実行する。
【０１３５】
したがって、ｎ自由度系の場合、入力するインパルスは計２ｎ本となり、これら全てのインパルスの入力位置が先に決まった。
【０１３６】
ここで、ｎ自由度系における１次モードからｎ次モードのエネルギ保存式は、次式(５７）となる。なお、ここでは詳細な計算は省力する。
【０１３７】
【数５７】
2014052763.tif 000059
【０１３８】
次に、式(５７）のうちのｎ次モードのエネルギ保存式より、Ａｉ＝αＡｉ−１となる奇数番号の入力の割合αは次式のように求まる。
【０１３９】
【数５８】
2014052763.tif 000060
【０１４０】
式(５７）のうちの（ｎ−１）次モードのエネルギ保存式より収束計算を行うと、偶数番号のインパルスの大きさＡｉは、次式のようになる。
【０１４１】
【数５９】
2014052763.tif 000061
【０１４２】
以上により、振動を抑制するインパルス入力２n本すべての大きさが理論的に求まる。したがって、ｎ自由度時変系の自由振動を抑制可能となった。
【０１４３】
上述の実施の形態の説明では、加速または減速運動を行う際に、運動体自体の姿勢の変化（例えば、慣性モーメントの変化）を伴う運動体において生じる、ｎ自由度かつ時変の振動を、インプットシェイピング法を利用して抑制する方法について説明した。このような運動体の代表的な例であるロボットアームでは、その運動動作の自由度を高めるために、多関節の自由度の高い構造が採用されている。そのため、現実的には、ロボットアームが駆動される場合には、複数の自由度かつ時変の振動が発生する。一方、このような運動体において、最も問題となるのは低周波振動である。そのため、このように複数の自由度についての時変の振動が生じるような場合には、発生する低周波振動から高周波振動の内で、問題となる低周波振動（例えば、３自由度系の場合、１次モードと２次モードの振動）に対してのみ、本実施の形態の振動抑制方法を適用しても良い。
【０１４４】
また、本実施の形態の振動抑制方法は、運動体が加速運動または減速運動を行うことにより生じる振動に対して適用することができるが、定速運動中において運動体の姿勢が変化することによりコリオリの力が仕事を行うような場合には、このような定速運動を行うことにより生じる振動に対しても適用できる。
【０１４５】
また、本実施の形態では、面内方向の動きが行われる場合を例として、主に回転運動が行われる場合について説明したが、回転運動以外の運動にも適用することができる。
【産業上の利用可能性】
【０１４６】
本発明によれば、加速または減速運動に伴って運動体に生じる、ｎ自由度（ｎ≧２）かつ時変の振動をインプットシェイピング法を用いて抑制する場合に、（ｎ−ｋ）次モードの振動を抑制するため、入力位置が設定されたインパルスＡ０・・・Ａ２k−１から（ｎ−ｋ）次モードの固有周期の半周期経過後に入力するように、インパルスＡ２k・・・Ａ２k+1−１の入力位置をそれぞれ設定し、運動体自体の姿勢変化に伴って生じるコリオリの力がする仕事を考慮して、前記インパルスＡ２k・・・Ａ２k+1−１の大きさをそれぞれ設定することを、整数ｋ＝０から（ｎ−１）まで順に実行することによって、入力位置と大きさがそれぞれ設定された合計２ｎ本のインパルスＡ０・・・Ａ２n−１を重ね合わせることで、効果的に振動を抑制することができる。
【０１４７】
したがって、例えばロボットアーム、工作機械、クレーンおよび油圧ショベルなどの、その動作中に姿勢変化を伴うような動作を行う産業用機械装置に生じる振動を、フィードフォワード制御にて効果的に抑制することができる。
【０１４８】
また、上記運動体の駆動源は、電動回転機や電動直動機に限定されない。すなわち、本発明は、動力発生機が油圧ポンプなどの非電動型の動力発生機である場合でも、支障なく適用可能である。
【０１４９】
なお、上記様々な実施の形態のうちの任意の実施の形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。
【符号の説明】
【０１５０】
１０ ロボットアーム（運動体）、１１ａ〜１１ｃ 関節、１２ａ〜１２ｂ アーム、１３ ハンドツール、１４ａ〜１４ｃ 回転軸、２０ モデル、２１ アーム、２２ 搬送物、２４ 駆動装置、Ａ０ 第１のインパルス入力、Ａ１ 第２のインパルス入力、Ａ２ 第３のインパルス入力、Ａ３ 第４のインパルス入力、ｑ１ １次モードのインパルス応答、ｑ２ ２次モードのインパルス応答
【図１】
2014052763.tif 000062
【図２】
2014052763.tif 000063
【図３】
2014052763.tif 000064
【図４】
2014052763.tif 000065
【図５】
2014052763.tif 000066
【図６】
2014052763.tif 000067
【図７】
2014052763.tif 000068
【図８】
2014052763.tif 000069
【図９】
2014052763.tif 000070
【図１０】
2014052763.tif 000071
【図１１】
2014052763.tif 000072
【図１２】
2014052763.tif 000073
【図１３】
2014052763.tif 000074
【図１４】
2014052763.tif 000075
【図１５】
2014052763.tif 000076
【図１６】
2014052763.tif 000077
【図１７】
2014052763.tif 000078
【図１８】
2014052763.tif 000079
【図１９】
2014052763.tif 000080
【図２０】
2014052763.tif 000081
【図２１】
2014052763.tif 000082
【図２２】
2014052763.tif 000083
【図２３】
2014052763.tif 000084
【図２４】
2014052763.tif 000085
【図２５】
2014052763.tif 000086