振動抑制方法
【課題】加速または減速運動に伴って運動体に生じるｎ自由度かつ時変の振動を、インプットシェイピング法を適用して効果的に抑制する振動抑制方法を提供する。
【解決手段】運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第１の入力に対する第２の入力の大きさを決定する。
【解決手段】運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第１の入力に対する第２の入力の大きさを決定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、加速または減速運動に伴って運動体に生じる振動を抑制する振動抑制方法に関するものであり、特に、運動中に運動体自体の姿勢が変化する場合に、運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法に関する。
【背景技術】
【０００２】
様々な作業を行うために運動動作を行うロボットアームは、モータとアームの間に減速機を用いている。減速機はねじりばねとして振る舞うため、モータ停止後もアームの振動が残留する。その結果、振動が収まるまで次の作業に移ることができず、作業効率を向上させることができない。
【０００３】
このような残留振動に伴う課題は、ロボットアームだけに限られず、加速または減速運動を含む運動動作を行う運動体、例えば、工作機械やクレーン等についても共通する。
【０００４】
フィードバック制御で振動を抑える場合は検出器が必要となるため、検出器が不要なフィードフォワード制御で振動を抑える方法が求められている。
【０００５】
フィードフォワード制御での振動抑制方法としては、インプットシェイピング法が知られている（例えば、非特許文献１参照）。このインプットシェイピング法は、工作機械などのように、固有振動数が変化しない場合に有効な振動抑制方法であり、様々な提案がなされている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−２９６１７号公報
【非特許文献】
【０００７】
【非特許文献１】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
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ロボットアームは、様々な作業に対応可能とするために、一般に多関節および多自由度を有している。そのため、ロボットアームにおいて、一の関節が駆動された運動動作を行っている間に他の関節が駆動されると、運動動作を行っているアーム自体の姿勢が変化する。そのため、アームの固有振動数が時間によって変化することになる。このような場合にあっては時変の振動系となるため、時不変の振動系を対象として従来のインプットシェイピング法を単に適用するだけでは、振動抑制に対応することができない。
【０００９】
このような時変の振動系において振動を抑制する方法としては、いくつかの提案がなされている（例えば、特許文献１および非特許文献２参照）。しかしながら、時変の振動抑制に対して、インプットシェイピング法を効果的に適用できる振動抑制方法が求められている。特に、自由度が２以上のロボットアームに対しては、複数の固有振動数が時々刻々と変化していくため、インプットシェイピング法を用いて多自由度かつ時変の振動を効果的に抑制することが求められている。
【００１０】
従って、本発明の目的は、上記課題を解決することにあって、加速または減速運動に伴って運動体に生じるｎ自由度かつ時変の振動を、インプットシェイピング法を効果的に適用して抑制する振動抑制方法を提供することにある。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、本発明は以下のように構成する。
【００１２】
本発明の第１態様によれば、加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法を提供する。
【００１３】
本発明の第２態様によれば、加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間の相互作用によるエネルギの移り変わりを無視し、加速または減速運動を行っている間に運動体自体の姿勢が変化することにより生じるコリオリがした仕事を考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法を提供する。
【００１４】
本発明の第３態様によれば、加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動におけるモード間の相互作用によるエネルギの移り変わりをゼロと見なして、固有振動数および減衰比の時間変化によるモード間のエネルギの移り変わりを考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法を提供する。
【００１５】
本発明の第４態様によれば、各振動モードにおいて、第１の入力に対応する入力を打ち消す第２の入力に対応する入力の大きさを個別に算出して、振動モードごとに算出された入力を合成することで第２の入力の大きさを決定する、第１態様から第３態様のいずれか１つに記載の振動抑制方法を提供する。
【００１６】
本発明の第５態様によれば、運動体が多関節型ロボットアームであって、ロボットアームに生じるｎ自由度かつ時変の振動を抑制する、第１態様から第４態様のいずれか１つに記載の振動抑制方法を提供する。
【発明の効果】
【００１７】
本発明によれば、加速または減速運動に伴って運動体に生じる、ｎ自由度かつ時変の振動を、インプットシェイピング法を用いて抑制する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第２の入力を決定するという手法を採用している。そのため、第１の入力によって生じたｎ自由度かつ時変の振動（第１の応答）を抑制できる第２の入力を算出することが可能となる。よって、加速または減速運動に伴って運動体に生じるｎ自由度かつ時変の振動を、インプットシェイピング法適用して効果的に抑制する振動抑制方法を提供することができる。
【図面の簡単な説明】
【００１８】
【図１】本発明の実施の形態にかかる振動抑制方法が適用される多関節型ロボットアームの模式図
【図２】図１のロボットアームにおける関節の模式図
【図３】時変におけるインパルス応答の変位と時間の関係図
【図４】時変におけるインパルスの重ね合わせの考え方を示す図
【図５】２関節ロボットアームにおける各モードの振動形態を示す図
【図６】実施例１にかかる可変ばねを備える１自由度線形時変系の構成図
【図７】インパルス応答Ｈ(t,0)を示す図
【図８】追加入力と時間の関係図
【図９】オリジナル系の応答と近似式による応答との比較図（ａ＝π）
【図１０】参照加速度指令値と追加指令値との波形図
【図１１】整形した加速度指令値の波形図
【図１２】加速度指令値に対する応答の波形図
【図１３】整形された加速度指令値とＣｈｏ方法による加速度指令値の波形図
【図１４】残留振動の低減効果を示す図
【図１５】実施例２にかかる水平２関節型ロボットアームの構成図
【図１６】水平２関節型ロボットアームの実験モデルの上面図
【図１７】サーボドライバの設定値を示す表
【図１８】実験モデルの測定パラメータを示す表
【図１９】自由応答の測定方法の模式図
【図２０】２関節型ロボットアームのアクセラレンスを示す図
【図２１】実験モデルのパラメータを示す表
【図２２】実固有値解析により求まる固有振動数を示す図
【図２３】ロボットアームにおける座標系の定義を示す図
【図２４】２関節型ロボットアームの突き出し動作の模式図
【図２５】突き出し動作における指令値を示す図
【図２６】突き出し動作中の疑似固有振動数と疑似減衰比を示す図
【図２７】オリジナル系と近似式による応答との比較図
【図２８】近似における応答誤差を示す図
【図２９】整形後の加減速パターンを示す図
【図３０】ZV Shaperにより整形した加減速パターンを示す図
【図３１】突き出し動作におけるΣE座標系のアーム先端の変位を示す図（計算値）
【図３２】突き出し動作におけるΣE座標系のアーム先端の変位を示す図（実験値）
【図３３】突き出し動作におけるΣH座標系のアーム先端の変位を示す図（計算値）
【図３４】突き出し動作におけるΣB座標系のアーム先端の変位を示す図（計算値）
【図３５】三角形速度パターンの図
【図３６】台形速度パターンの図
【図３７】アーム先端の最大変位を示す図
【図３８】アーム先端の最大変位を示す図（計算値）
【図３９】アーム先端の最大変位を示す図（実験値）
【図４０】加減速パターン、速度パターン、指令値を示す図
【図４１】整形した加減速パターン、速度パターン、指令値を示す図
【図４２】理想指令角と、送信されるデジタル化された指令角を示す図
【図４３】２関節型ロボットアームの振り回し動作の模式図
【図４４】振り回し動作後のΣE座標系におけるアーム先端の変位を示す図（計算値）
【図４５】振り回し動作後のΣE座標系におけるアーム先端の変位を示す図（実験値）
【図４６】アーム先端の最大変位を示す図（計算値）
【図４７】アーム先端の最大変位を示す図（実験値）
【図４８】２関節型ロボットアームの折り畳み動作の模式図
【図４９】折り畳み動作後のΣE座標系におけるアーム先端の変位を示す図（実験値）
【図５０】残留振動のパワースペクトル図
【図５１】実施例３にかかる垂直２関節型ロボットアームの構成図
【図５２】垂直２関節型ロボットアームのパラメータを示す表
【図５３】参照指令値、加減速パターン、速度パターンを示す図
【図５４】整形後の参照指令値、加減速パターン、速度パターンを示す図
【図５５】突き出し動作後のΣB座標系におけるアーム先端の変位を示す図
【図５６】動作中の各関節の角速度を示す図
【図５７】釣り合い位置からの偏差角を示す図
【発明を実施するための形態】
【００１９】
以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。
【００２０】
（実施の形態）
本発明の実施の形態にかかる運動体の振動抑制方法について、運動体の一例であるロボットアーム１０を用いて説明する。
【００２１】
（多関節型ロボットアームの運動方程式）
図１に示すように、ロボットアーム１０は、多関節型ロボットアームであり、ｎ個のリンクＬ１，・・・，Ｌｎと、それぞれのリンクＬを動作可能に接続するｎ個の関節Ｊ１，・・・，Ｊｎとを備えている（ｎは自然数）。また、関節Ｊの部分を示す図２および図１に示すように、このロボットアーム１０のそれぞれのリンクＬおよび関節Ｊは、１つの平面内で動作可能となっている。
【００２２】
図２に示すように、関節Ｊ１の角度をθ1、その指令角をθ01、ばね定数をｋ1とし、関節Ｊ１以外の任意の関節ｊの角度をθj、その指令角をθ0j、ばね定数をｋjとする。上付き添え字Ｔは転置を表すとし、数１〜数３のように定義する。
【数１】
【数２】
【数３】
【００２３】
数１〜数３の定義と、ラグランジュの方法により、多関節型ロボットアーム１０の運動方程式は、数４のように表される。
【数４】
ここで、Ｊ（θ）は慣性行列、ｈ（左辺第２項）はコリオリの力と遠心力を表す項、ｇ（θ）は重力付加を表す項、ＣVは粘性減衰行列である。
【００２４】
（テイラー展開）
次に、非線形な式である数４をθ＝θ0まわりの１次のテイラー展開により線形化を行うと、数５が得られる。なお、数５におけるφ、Ｃh、Ｋm、Ｋh、Ｋgは、数６〜数１０の通りである。
【数５】
【数６】
【数７】
【数８】
【数９】
【数１０】
【００２５】
（多自由度線形時変系のモード分解）
モード解析は、本来は線形時不変系を対象とする解析手法の一つである。ここでは、このモード分解を連続時間で記述される線形時変系に適用し、入力値整形に利用する。
【００２６】
一般にｎ自由度線形時変系の運動方程式は、ｎ次正方実行列の質量行列Ｍ(t)、減衰
行列Ｃ(t)、剛性行列Ｋ(t)、ｎ行縦ベクトルの入力ｆ(t)、偏差角ベクトルφ(t)を用いて、数１１のように表せる。
【数１１】
【００２７】
多関節型ロボットアーム１０の場合には、指令値θ0に対して参照指令値θref(t)を与えるものとして、数１２〜数１５とすれば、数５は数１１の形の多自由度線形時変系とみなすことができる。ただし、ｈref(t)、ｇref(t)は数１６、数１７のように表される。
【数１２】
【数１３】
【数１４】
【数１５】
【数１６】
【数１７】
【００２８】
ここで釣り合いの位置からの偏差角ψは数１８の通りである。
【数１８】
この時間に関する１階微分と２階微分はそれぞれ数１９、数２０のようであるので、釣り合いの位置からの偏差角ψに関する運動方程式は数２１のようになり、数２２のように表される。
【数１９】
【数２０】
【数２１】
【数２２】
【００２９】
Ｅｎをｎ次単位行列とすると、数２１の運動方程式に対応する状態方程式は、数２３〜数２６にように表される。
【数２３】
【数２４】
【数２５】
【数２６】
【００３０】
ここで、Ａの時刻ｔにおける瞬間的な固有ベクトルと固有値について、常に共役なペアがｎ個存在する場合、つまり、全モードの応答が振動的であり、過減衰または臨界減衰でない場合についてのみ考える。Ａの時刻ｔにおけるｉ番目の瞬間的な固有ベクトルをｐi(t)、対応する瞬間的な固有値をλi(t)とし、固有ベクトルからなる行列Ｐ(t)と固有値を対角要素にもつ行列Λ(t)を、数２７、数２８のように定義する（ｉは自然数）。ただし、数２９の関係式が成立するようにｉを選ぶものとする。
【数２７】
【数２８】
【数２９】
【００３１】
ここで、モード座標ｑを数３０のように定義する。すると、状態方程式はモード座標について、数３１のように表すことができる。
【数３０】
【数３１】
ここで、数３１に示される状態方程式における右辺第一項である数３２にて表される行列のうち対角要素のみを取り出した行列Ｄ(t)を考え、この行列Ｄ(t)を数３３のように表す。
【数３２】
【数３３】
数３２の行列において、非対角要素の絶対値が対角要素の絶対値に比べて十分に小さいことから、数３３の行列Ｄ(t)を用いて、数３１の状態方程式を数３４のように近似できる。なお、この状態方程式の近似の考え方については後述する。
【数３４】
【００３２】
数３１の状態方程式では、数３２の行列において、非対角要素に有意な値があるため、このままでは方程式を解くことができない。しかしながら、数３１の状態方程式を数３４の状態方程式にように近似することで、各モードの応答を独立な方程式を解くことで求めることが可能となる。これにより、ｉ次モードのインパルス応答関数を数３５のように導くことができる。なお、τは単位インパルスを与える時刻である。
【数３５】
【００３３】
（モードごとの入力値整形）
次に、ロボットアーム１０に付加された第１の入力によってロボットアーム１０に生じる第１の応答に、ロボットアーム１０に第２の入力を付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うような第２の入力の値を、振動モードごとに算出方法について説明する。
【００３４】
ここで、インプットシェイピング法におけるインパルス応答の重ね合わせの考え方について、図３に示す。図３ではインパルス応答の変位（振動量：縦軸）と時間（横軸）との関係を示している。
【００３５】
図３に示すように、ある時刻τに与えられる第１のインパルスＰ１（第１の入力）に対する第１の応答Ｈi(ｔ,τ)（第１の応答）を打ち消すため、第２のインパルスＰ２（第２の入力）を与える時刻をτ~（数式上では、「τ」の上に「~」と表す。以降同じ。）（＞τ）とし、第１のインパルスＰ１に対する第２のインパルスＰ２の大きさの比を実数α~（数式上では、「α」の上に「~」と表す。以降同じ。）とする。ここで、α~とτ~は未知数であり、これらは以下のように求めることができる。
【００３６】
まず、第１のインパルスＰ１により生じる第１の応答とインパルスＰ２により生じる第２の応答とが、ｔ＞τ~において互いに打ち消しあうための条件は、数３６にて表される。
【数３６】
数３５より、ｔ＞τ~では、数３７の関係式が成り立つため、数３６は数３８のようになる。
【数３７】
【数３８】
一般にＨi(ｔ,τ~)≠０であるので、数３６を満たすための条件は、数３９のようになる。
【数３９】
この数３９は、第２のインパルスＰ２の大きさを求める式である。τ~は未知であり、α~は実数であるので、数４０に示す関係を満たさなければならない。
【数４０】
数４０を用いて、時刻τで与えられた第１のインパルスＰ１に対する第１の応答の虚部が０になる時刻τ~を求めることができる。しかし、数４０を満たすτ~は無数に存在する。そこで、数４０を満たす最小のτ~をτ'とする。なお、τ'は力学系においては、第１のインパルスＰ１によって生じる第１の応答に対して、変位が最初に０になる時刻である。また、(τ'−τ)は、時不変系で言うところの半周期に相当する。時変系であれば、τ'は第１のインパルスＰ１を与える時刻τにより異なるので、τ'をτの実関数として、数４１にように表す。
【数４１】
さらに、αi(τ)を数４２のように定義すれば、数４３の関係式が成り立つ。
【数４２】
【数４３】
【００３７】
次に、このインパルスの組み合わせを用いて、ｉ次モードに対する任意の入力ｕi(ｔ)による応答を打ち消すことができる追加の入力ｗi(ｔ)を求める。任意の入力ｕi(ｔ)はある時刻τSからτEの間にのみ任意の複素数をとるものとし、それ以外の時刻(ｔ＜τS，ｔ＞τE）では０とする。数４３にｕi(ｔ)を乗じ、τについてτSからτEまで積分すると、τ＞ηi（τE）のとき、数４４が成り立つ。
【数４４】
数４４の左辺第一項は畳み込み積分の形になっており、入力ｕi(ｔ)に対する応答を表している。左辺第二項についても畳み込み積分の形にするため、積分変数をτからτ'に変換する。なお、ηiの逆関数をηi-1、ηi-1の１次導関数を数４５のように表すと、数４６、数４７のようになる。
【数４５】
【数４６】
【数４７】
これらより、数４４は数４８のように表される。ただし、数４９〜数５１のような関係になる。
【数４８】
【数４９】
【数５０】
【数５１】
【００３８】
数４８の左辺第一項は、ｕi(ｔ)による応答、左辺第二項はｗi(ｔ)による応答を表すことになる。さらに、これらの足し合わせがｔ＞τ'Eにおいて０になることを、数４８は示している。つまり、ｕi(ｔ)＋ｗi(ｔ)をｉ次モードに対する第２の入力とすれば、このｉ次モードに生じた残留振動は抑えられることになる。なお、第１のインパルスＰ１が与えられた時刻τから第１の応答の虚部が最初に０になるまでの時間（τ'−τ）を時変系における半周期と呼ぶことにすれば、数５１においてηi-1(τ')は、時刻τ'から見て半周期前の時刻τを表すことになる。また、αi(ηi-1(τ'))は時刻τに与えられた第１のインパルスＰ１に対する第１の応答が、半周期の間にどれだけ減衰するのかを表しており、ｕi(ηi-1(τ'))は半周期前の入力の大きさを表すことになる。また、数５２は、時変系における固有周期の変化に対する補正量である。
【数５２】
ここで、時変系において連続的に第１のインパルスが入力される場合を考える。図４に示すように、第１の入力として、連続的にインパルスが入力された第１のインパルス列Ｐ３により生じる第１の応答を打ち消すように連続的にインパルスが入力された第２のインパルス列Ｐ４を考える。時変系の場合は、第１のインパルス列Ｐ３の時間間隔がｄτであるのに対して、第２のインパルス列Ｐ４の時間間隔は数４８における積分変数の変換により数５３のように表される。そのため、この時間間隔を補正するために追加の入力に対しては、数５２に示される時変系における固有周期の変化に対する補正量を乗じる必要がある。
【数５３】
【００３９】
ここで、元々の入力ｕi(ｔ)に対してｗi(ｔ)を足し合わせると、例えばこの入力を力とみれば、系に加わる力積が単純に増えるということになる。そこで、入力の正規化係数を数５４で定義する。
【数５４】
数４８に任意の複素定数を乗じても右辺が０となることから、ｕi(ｔ)＋ｗi(ｔ)に任意の複素定数を乗じても２つの入力は打ち消し合うことになる。そこで、数５４で定義される正規化係数を用いて、各モードにおける整形された入力値は、数５５のように表すことができる。
【数５５】
【００４０】
（複数モードを対象とした入力値整形）
次に、複数モードを対象とした入力値整形の方法を示す。まず、入力ｆとして任意の参照入力ｆrefを与えると、対応する各モードへの入力ｕiは、数５６のように表される。
【数５６】
この数５６をもとに数５５により整形された入力ｕ'i(ｔ)をモードごとに求め、任意の実定数κ1,・・・,κnを用いて、数５７のように表す。
【数５７】
Ｂ＋をＢの疑似逆行列とすると、数５８で表されるｆshapedが、数５６で表される多自由度線形時変系において、全てのモードの残留振動を抑制する入力となる。κiｕ'iがｉ次モードの残留振動を抑制する入力であり、さらにその他のモードを励起しないため、そのような入力の足し合わせであるｆshapedは、結局、全てのモードについての残留振動を抑制することになる。
【数５８】
【００４１】
（加減速パターンの生成）
次に、ロボットアーム１０の残留振動を抑制する加減速パターン（数５９）を生成する方法について説明する。本手法により加減速パターンを整形した結果、対応する速度パターン（数６０）、変位指令値（数６１）は、（数６２）、（数６３）から大きく変わらないものとする。これにより、加減速パターンを整形した後もモード形状の時間推移は変わらないものとみなせる。すると、数１５、数２２より、数６４で表される加減速パターン（数５９）が残留振動を抑制する加減速パターンとして生成される。ただし、κ＝[κ1・・・κn]は動作終了時に、対応する速度パターン（数６０）が０となるように、信頼領域Dogleg法によりκを変化させながら数５６〜数５８、数６４を繰り返し評価することにより決めることが望ましい。また、Γは対角実行列であり、整形後の指令値（数６１）が参照指令値（数６３）と同じ値で終わるようにその要素を決めることが望ましい。
【数５９】
【数６０】
【数６１】
【数６２】
【数６３】
【数６４】
【００４２】
（状態方程式の近似について）
ここで、上述した数３１の状態方程式において、数３２にて表される右辺第一項の行列のうちの非対角要素を無視して（すなわち０として）、対角要素のみを取り出した行列Ｄ(t)を用いて、数３４のように近似した本発明の考え方について、さらに説明する。
【００４３】
２関節ロボットアーム２０を例として説明する。図５に示すように、２関節ロボット２０では２種類の振動の形態が考えられる。図５（ａ）に示す振動の形態が１次モードであり、図５（ｂ）に示す形態が２次モードである。このような振動の形態は、１次モードについては固有ベクトルｐ１によって決まり、２次モードについては固有ベクトルｐ２によって決まる。実際には、この２つの振動モードが合成されて（混在して）アームが振動し、２つの振動の形態が合成された動きとなる。
【００４４】
時変系の場合は、数３１において、行列（数３２）の非対角要素が０でないため、ｑ1の値がｑ2に影響を及ぼし、ｑ2の値がｑ1に影響を及ぼすことになる。すなわち、時変系の場合には振動モード間での相互作用が発生することになる。これに対して、時不変系の場合には振動モード間での相互作用は発生しない。時変系におけるこの相互作用の大きさが行列（数３２）の非対角要素の値の大きさと対応する。
【００４５】
しかしながら、運動体がロボットアームの場合には、振動モード間での相互作用はあまり大きくない。具体的には、行列（数３２）の対角要素（モード毎の自らの時変性が示されている要素）に比して、非対角要素（モード間の相互作用の大きさが示されている要素）が十分に小さい。そこで、この非対角要素を無視して０とみなし、数３２の対角要素のみを取り出したＤという行列（数３３）を用いることで、数３１の状態方程式を数３４のように近似することで、モードごとの応答を独立な方程式を解くことで算出することを可能とするというのが、本発明の考え方である。なお、このような観点からは、本発明において「振動モード間の相互作用」とは、数３１において、ｑ1の値がｑ2に影響を及ぼし、ｑ2の値がｑ1に影響を及ぼす作用を意味すると言うことができる。
【００４６】
また、モード間のエネルギのやり取りの観点からも、この相互作用について説明できる。系に減衰が無く、外力も与えられていないとすれば、エネルギ保存則が成り立つ。１次モードのエネルギをＥ1、２次モードのエネルギをＥ2とすれば、Ｅ1＋Ｅ2は常に一定の値となる。時不変系であれば、さらにモードごとにエネルギ保存則が成り立つ。つまり、Ｅ1とＥ2がそれぞれ一定の値となる。これに対して、時変系では、モードごとにエネルギ保存則が成り立たず、モード間でエネルギのやり取りが発生する。例えば、Ｅ1が大きくなるときには、Ｅ2がその分だけ小さくなる。逆に、Ｅ1が小さくなるときには、Ｅ2はその分だけ大きくなる。
【００４７】
ここで、このような時変系におけるモード間でのエネルギのやり取りには２つの要因がある。１つ目は、行列（数３２）の対角要素、つまり固有振動数と減縮比が時間により変化することである。２つ目は、行列（数３２）の非対角要素により異なるモード間にて相互に作用し合うことによるものである。本発明では、時変系において、１つ目の要因を考慮しながら、２つ目の要因を無視することにより、数３４のような近似式を得るという考え方に基づいている。
【００４８】
すなわち、本発明の振動抑制方法では、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第１の入力により生じる第１の応答を打ち消すような第２の入力の大きさを決定する、という考え方に基づいている。
【００４９】
次に、このような考え方に基づく、ｎ自由度かつ時変の振動を抑制する振動抑制方法について、具体的な実施例を用いて説明する。
【００５０】
（実施例１：１自由度線形時変系）
まず、実施例１として、１自由度線形時変系について説明する。具体的には、上記実施の形態にて説明した手法を、図６に示す可変ばねを有する１自由度振動系に適用し、本手法の時変系に対する有効性を確かめる。特に固有周期の変化に対する補正が、高速にパラメータが変化する際に重要となることを本実施例１にて示す。
【００５１】
（運動方程式）
図６に示す可変ばねを有する１自由度振動系を考える。ばねの自然長は一定である。質量は１に正規化し、図示右向きを正として質量の位置をξとする。固有振動数をωn(t)とすると、ばね定数はωn2となる。また、右向きを正として指令値をξ0とする。すると、この系の運動方程式は数６５で表される。
【数６５】
ここで、質量の変位と指令値の偏差ｚ＝ξ−ξ0について運動方程式を書きなおすと、数６６で表される。なお、初期条件は数６７にて表される条件とする。
【数６６】
【数６７】
この偏差に関する運動方程式に対応する状態方程式は、数６８のように表される。
【数６８】
数６８において、Ａ(t)の時刻ｔにおける瞬間的な固有ベクトｐ(t)と固有値λ(t)はそれぞれ数６９、数７０にて表される。
【数６９】
【数７０】
【００５２】
（モード座標への変換）
次に、モード座標への変換を行う。数７１および数７２とすれば、数７３にて表されるモード座標を用いて、状態方程式は数７４と書き換えられる。
【数７１】
【数７２】
【数７３】
【数７４】
ここで、数７５に表されるようにし、数７４の非対角要素を無視すれば、数７４は数７６のように近似できる。
【数７５】
【数７６】
各モードへの入力を数７７に示すようにすると、モードごとの運動方程式は、数７８にて表すことができる。
【数７７】
【数７８】
ここで、数７９とすれば、ｄの時間積分は数８０と書けるので、インパルス応答関数は、数８１と求まる。
【数７９】
【数８０】
【数８１】
【００５３】
（入力値整形）
次に、数４０を満たす数８２の最小値をτ'とすると、数８３となる。
【数８２】
【数８３】
これをτ'とτについて解き、それぞれτ'＝η(τ)，τ＝η−１(τ')とする。また、α(τ)＝−Ｈ(η(τ),τ)とする。数５１より、数８４により励起される振動を打ち消す加速度指令値は、数８５のように求めることができる。
【数８４】
【数８５】
【００５４】
（シミュレーション）
シミュレーションの例として、固有振動数が数８６のようになる場合について考える。ここで、ω0(＞０)はｔ＝０における初期固有振動数、ａ(＞０)は固有振動数の時間変化率を表す実定数である。すると、各種関数が、数８７〜数９１のように、初等関数のみを使って求めることができる。
【数８６】
【数８７】
【数８８】
【数８９】
【数９０】
【数９１】
参考のため、ａ＝π，ω0＝πの場合の数９０を図７に示し、数８８、数８９および数９１を図８に示す。
【００５５】
ここで、初速度０の状態から数９２の状態まで加速するための参照加速度指令値として、数９３を与えるものとする。
【数９２】
【数９３】
非対角要素を無視した数７６の近似の妥当性を調べるため、数９３を入力として与えたとき、数７４による応答ｑと近似式である数７６による応答ｑを、それぞれルンゲクッタ法の一種であるDormand-Prince法により図９のように得た。図９に示されるように、２つの応答はよく似ており、数７６の近似は妥当であると言える。
【００５６】
参照加速度指令値（数９３）が与えられれば、数８５を用いると、初等関数のみを用いて明示的に振動を打ち消す加速度指令値（数８５）を求めることができる。このようにして求めた振動を打ち消す加速度指令値を図１０に示す。ここで、残留振動を抑えつつ、加速期間終了時ｔ＝η(１)に速度（数９４）が１ｍ／ｓとなるように、加速度指令値を数９５とする。
【数９４】
【数９５】
参照加速度指令値（数９６）、整形後の加速度指令値（数９７）、Ｃｈｏの方法により整形した加速度指令値を図１１に示し、それぞれに対する応答を図１２に示す。図１２より明らかなように、本発明の手法で残留振動が大幅に抑制されていることが分かる。なお、Ｃｈｏの方法については参考文献（Cho, J. K. and Park, Y. S., “Vibration reduction in flexible systems using a time-varying impulse sequence”, Robotica, Vol. 13, Issue 3 (1995), pp. 303-313.）に開示されている。
【数９６】
【数９７】
【００５７】
特に着目すべき点は、本発明の手法(Proposed method)では追加の入力に相当する部分がＣｈｏの方法に比べて大きい点である。これは、時間が経つにつれて固有周期が短くなっているためである。このような場合には参照入力と追加の入力が与える力積を等しくするために、追加の入力の方を大きくなるよう補正する必要がある。本発明の手法では、この固有周期の変化に対する補正を数５２で示す項で行っている。一方で、図１３に示すように、Ｃｈｏの方法により得られる加速度指令値は数５２で示す項を１として整形した加速度指令値と一致することから、Ｃｈｏの方法では固有周期の変化に対する補正がされてないと言える。このことから、固有周期の変化に対する補正が時変系の残留振動抑制に必要であることが分かる。
【００５８】
また、数７５の積分が解析的に求まり、さらに、数８３が解析的に解けるのであれば、本発明の手法では単純な演算のみで入力値整形を行うことができる。実際、固有振動数の変化が数８６のように表せるため、初等関数のみを用いて入力値整形を行うことができた。このように、１自由度時変振動系を対象とする場合には、本発明の手法による入力値整形の演算量は少なくて済む。このため、多くの行列演算を必要とするＣｈｏの方法と比べて、本発明の手法は実機に実装するにあたって有用である。
【００５９】
（高速時変系に対する効果）
本発明の手法の特徴は、パラメータが高速に変化する場合にでも適用可能な点にある。先に説明した例を用いて、固有振動数の変化率ａが大きい場合でも残留振動が低減可能であることを数値計算によるシミュレーションにより示す。
【００６０】
まず、数９８に対する応答の絶対値|ｚ|のｔ＞１における最大値をｚrefmaxとする。また、数９９に対する応答の絶対値|ｚ|のｔ＞η(１)における最大値をｚmaxとする。すると、ｚmax／ｚrefmaxが入力値整形によりどれだけ残留振動が低減されたかを表す指標になる。そこで、ａをω0／１０から１０ω0の間で変えながら運動方程式をDormand-Prince法により解いた。どのようにｚmax／ｚrefmaxが変化するのかを、Ｃｈｏの方法による結果とともに図１４に示す。
【数９８】
【数９９】
【００６１】
図１４に示すように、本発明の手法では、固有振動数が初期固有振動数ω0に対して単位時間あたりで１０倍も変化するような場合でも、残留振動が３％以下にまで低減されていることが分かる。また、Ｃｈｏの方法と比べると、固有振動数が高速に変化する場合でも効果的に残留振動が抑制されていることが分かる。
【００６２】
（効果）
本発明の手法では、多自由度の複雑な系であっても、入力値整形に必要な関数を補間などの方法により求めることで計算時間の短縮を狙うことができる。α(t)は減衰などによるエネルギ消散量に関連しており、インパルス応答が分かれば容易にα(t)の関数形状を想像することができる。例えば、図７より、振幅が時間とともに増大していく場合には、対応するα(t)は常に１より大きくなり、振幅の変化が小さくなるとα(t)は１に近づいていく。また、η−１(t)についても瞬間的な固有周期の時間変化率に関連している。図７に示したように固有周期が短くなる場合には、２つ目のインパルスの値は大きくなるようにせねばならず、η−１(t)は常に１以上となる。また、固有周期をＴ(t)（数１００）とすると、その変化率は数１０１のように表され、時間の経過とともに固有周期の変化が小さくなることが分かる。
【数１００】
【数１０１】
このことから、時間の経過ともにη−１(t)が１に収束していくことが予想され、実際、図８ではそのようになっている。このように、固有周期の変化をあらかじめ知ることができればη−１(t)の形状を予測できる。すると、これらの関数は補間などの手法により得ることができ、設計者が補間に必要なだけの点数を選ぶことが可能になる。関数の形状さえ再現できるのであれば、不必要に時間間隔を細かく区切る必要が無くなる。このことから、本発明の手法では入力値を整形するのに必要な演算量（作業量）を低減することが可能であると言える。
【００６３】
（実施例２：水平２関節型ロボットアーム）
次に、実施例２として、水平２関節型ロボットアームへの適用例について説明する。
【００６４】
図１５に示す水平面内で動作する２関節ロボットアーム２０について、残留振動を抑制するための加減速パターンを生成し、数値計算と実験により上述にて説明した入力値整形の有効性を検証した。
【００６５】
この検証にて用いた実験装置（実験モデル）を図１６に示す。この実験装置は、第１リンク１１を水平面内にて回動させる第１関節２１(Joint 1)にＡＣサーボモータRSF-14A-100(ハーモニック・ドライブ・システムズ社)を用い、第２リンク１２を水平面内にて回動させる第２関節２２(Joint 2)にＡＣサーボモータRSF-14A-50(同社)を用いた水平面内で動作する２関節ロボットアーム２０である。これらのＡＣサーボアクチュエータには波動歯車装置が用いられている。各関節のモータＭ１、Ｍ２は、それぞれＡＣサーボドライバHA-655(ハーモニック・ドライブ・システムズ社)により図１７の表に示す設定で位置制御モードにより制御されており、外部指令装置からパルスを送ることで駆動する。第１関節２１は１パルスあたり4.5×10−3度、第２関節は9.0×10−3度回転する。外部指令装置にはDS1104 R&D コントローラボード(dSPACE 社)のDAC出力を用い、１秒あたり最大で2.5×104パルスを同時に２つのＡＣサーボドライバに送ることができる。また、動作終了後のアーム先端の残留振動の大きさの測定には、レーザー変位計HLC105B-HK(SUNX 社)を用いた。なお、この測定値はフィードバックすることなく、測定のためだけに用いた。
【００６６】
（運動方程式）
関節ｊの回転角度をθj，その指令値をθ0j，リンクｌの質量をｍｌ，リンクｌの関節ｊ周りの慣性モーメントをＪj，リンクｌの長さをｂｌ，関節ｊからリンクｌの質量中心までの距離をｌj，関節ｊのねじり剛性をｋjとする。運動方程式は、数１０３〜数１１１の条件に基づいて数１０２のように表される。
【数１０２】
【数１０３】
【数１０４】
【数１０５】
【数１０６】
【数１０７】
【数１０８】
【数１０９】
【数１１０】
【数１１１】
【００６７】
数５〜数１０によるテイラー展開により、参照指令値をθref＝ [θref1 θref2]Tとすると、水平２関節型ロボットアームの運動方程式は数２１で表される。ただし、各パラメータは数１２〜数１５より、数１１２〜数１１５のようになる。ただし、数１１６〜数１２２に示す関係にある。
【数１１２】
【数１１３】
【数１１４】
【数１１５】
【数１１６】
【数１１７】
【数１１８】
【数１１９】
【数１２０】
【数１２１】
【数１２２】
なお、水平多関節型ロボットアームの場合には重力項が無いため、数１８よりψ＝φである。
【００６８】
（パラメータ同定）
本発明の手法により加減速パターンを生成するために必要なパラメータの同定方法について述べる。まず、電子はかりによる測定と幾何形状から求まるパラメータを図１８の表に示す。ここで、未知のパラメータはＪ１、ｋ１、ｋ２、Ｃvである。これらは以下に説明するようにして同定した。
【００６９】
（モーダルパラメータの同定）
まず、全てのモータをサーボオンにした状態で、第１関節２１に対する指令角をθ01＝０，θ02＝θr2として固定したときの自由応答を測定した。ここで、θr2は、数１２３のように表される。
【数１２３】
自由応答は、図１９のように、加速度ピックアップをアーム先端から9mmのところに貼り付け、その裏側をインパクトハンマにより加振し、アーム先端の加速度を測定した。θ02＝θr2のときにＦＦＴアナライザにより得られたアクセレランスをＧr(ωk)とする。なお、サンプリング周波数を256Hz，計測時間を16秒，ωk＝2πｆk，ｆkを0，0.0625，0.125，...，128Hzとした。ここで、一般粘性減衰をもつ２自由度系のアクセレランスＬr(ω)（数１２４）に対し、Ｇr(ωk)を1〜100Hzの領域で曲線適合させるため、数１２５にて表される値が最小となるようパラメータＵri，Ｖri，ωrni，ζriを信頼領域Reflective法により求めた。
【数１２４】
【数１２５】
【００７０】
一例として、θ02＝θ42＝π/4radのときに計測した伝達関数Ｇ4(ωk)と、求まったパラメータをもとに求めた伝達関数Ｌ4(ω)を図２０に示す。
なお、θ02＝π/2，7π/12radのときは、鉛直方向の１次固有振動数と水平方向の２次固有振動数が近く、インパクトハンマによる打撃によって鉛直方向の振動が少なからず励起されるとともに、加速度ピックアップが鉛直方向の加速度をも拾ってしまうため、曲線適合は一般粘性減衰をもつ３自由度系のアクセレランスＬr(ω)（数１２６）を用いて行った。
【数１２６】
【００７１】
（慣性モーメントとねじり剛性の同定）
次に、ωrniをｉ次モードの固有振動数とみなし、このパラメータをもとにＪ1，ｋ1，ｋ2を以下のように同定した。数５において数１２７に示すようにしたときの偏差角φに関する運動方程式（数１２８）より実固有値解析によって求まる固有振動数をω'rniとする。
【数１２７】
【数１２８】
そして、数１２９に示す値が最小となるよう信頼領域Reflective法によりＪ1，ｋ1，ｋ2を図２１に示す表のように同定した。
【数１２９】
アクセレランスの曲線適合により求めた固有振動数ωrniと、同定したＪ1，ｋ1，ｋ2を運動方程式（数１３０）に代入し、実固有値解析により求まる固有振動数ω'ni(θ2)を図２２に示す。
【数１３０】
なお、第１関節２１に用いられているモータＭ１のねじり剛性は、ねじり角が小さい場合には4.7×103N・m/radである。また、第２関節２２では3.4×103N・m/radである。同定したねじり剛性ｋ1，ｋ2がこれらの値よりも小さいのは、関節に用いられている軸の軸径が小さいことにより機械的剛性が低くなっている部分があることと、サーボ機構により系全体の剛性が下がっていることによる。
【００７２】
（粘性減衰行列の同定）
次に、粘性減衰行列Ｃvを求める。ζriをｉ次モードのモード減衰比とみなし、θ02＝θr2のときの粘性減衰行列が、定数αr，βrを用いて数１３１というRayleigh減衰の形で与えられるとすると、定数αr，βrは、ｉ次モードに関して数１３２という関係を満たす。これより、１次のモーダルパラメータωrn1，ζr1と、２次のモーダルパラメータωrn2，ζr2を用いて、定数αr，βrがそれぞれ数１３３、数１３４と求まる。
【数１３１】
【数１３２】
【数１３３】
【数１３４】
数１３１、数１３３、数１３４よりＣvrが求まるので、その平均をとることで数１３５のようにＣvを決めた。
【数１３５】
【００７３】
（座標系の定義）
アーム先端の面内振動を評価するため、図２３のようにベースから見た座標系ΣB，アーム先端の指令位置から見た座標系ΣH，動作終了後のアーム先端の位置から見た座標系ΣEを定める。なお、θE＝[θ1E θ2E]Tは動作終了時における関節の角度である。
【００７４】
図２３より、座標系ΣBにおけるアーム先端の位置は、数１３６、数１３７のようになる。
【数１３６】
【数１３７】
座標系ΣHにおけるアーム先端の位置は、同次変換（数１３８）により、数１３９、数１４０となる。
【数１３８】
【数１３９】
【数１４０】
この座標系ΣHは動作中のアーム先端の振動を評価するために用いる。同様にして，座標系ΣEにおけるアーム先端の位置は、数１４１、数１４２となる。
【数１４１】
【数１４２】
座標系ΣEにおけるアーム先端の位置は、レーザー変位計により直接測定できるもので、動作終了後のアーム先端の残留振動を評価するために用いる。
【００７５】
（突き出し動作）
次に、図２４に示す突き出し動作について考える。動作開始時の各関節の角度θSは数１４３にて表され、動作終了時の関節の角度θEは数１４４にて表される。
【数１４３】
【数１４４】
【００７６】
最大角加速度を4πrad/s2，最大角速度をπrad/sとし、２つの関節が同時に動作終了するようにすると、最短時間制御は図２５に示すような指令値となる。これを参照指令値θrefとし、対応する加減速パターン（数１４５），速度パターン（数１４６）についても図２５に示す。
【数１４５】
【数１４６】
【００７７】
ここで、ｉ次モードの疑似固有振動数と疑似減衰比をそれぞれωi(t)＝|di|，ζi(t)＝−real[di]/ωiとすると、動作中の疑似固有振動数と疑似減衰比は図２６のように変化する。１次固有振動数と２次固有振動数は、動作開始時にはそれぞれ7.9 Hz，18.3 Hzであるが、動作終了時にはそれぞれ6.2 Hz，28.0 Hz となる。
【００７８】
（近似の妥当性）
この突き出し動作において、上述にて説明した加減速パターンを導出する際に行った近似の妥当性を検証する。元々の非線形な運動方程式（数４）、剰余項を無視して線形化した運動方程式（数２１）、非対角項を無視することによって導いたモードごとの非連成な運動方程式（数３４）をそれぞれ数値計算により解いた。その結果得られる偏差角をそれぞれψ＝[ψ1 ψ2]T，ψL＝[ψL1 ψL2]T，ψM＝[ψM1 ψM2]Tとし、図２７に示す。また、非線形な運動方程式（数３４）の解ψからの誤差を数１４７、数１４８のように定義し、図２８に示す。
【数１４７】
【数１４８】
なお、数値計算にはDormand-Prince法を用いた。図２８より、線形化したことによる誤差が５％以下であり、モード分解をしても誤差が５．５％以下であることから、この突き出し動作における数３４の近似は妥当であると言える。
【００７９】
（加減速パターンの生成）
加減速パターンを生成するにあたって、数１４９を解析的に求めることは困難であるため、差分を用いて数１４９を計算する。しかしながら、図２５に示す加減速パターンは時刻ｔに関して不連続であるため、微小時間ΔtPを用いて数１５０とすると、Ｐ(t)が不連続になる時刻ｔにおいて、数１５１の各要素が他の時刻のときと比べて非常に大きな値をとり、生成される加減速パターンは残留振動を抑制しなくなる。
【数１４９】
【数１５０】
【数１５１】
そこで、数１５１を数１５２とする。
【数１５２】
ただし、med関数は引数として与えられた行列のうち、フロベニウスノルムが中央値をとる行列を返す関数である。このように数１４９を定義しなおすと、ΔtPを小さくしても、Ｐが不連続な点において数１４９の各成分が異常に大きな値をとることがなくなる。
【００８０】
この数１４９を用いて、数６４により得られる加速度パターン（数５９）と対応する（数６０）（数６１）を図２９に示す。ただし，ΔtP＝0.001sとした。整形前の図２５と整形後の図２９とを比べることにより、両者の速度パターンと指令値に大きな差異がないことが分かる。よって、この場合にはモード形状の時間推移は整形後も変わらないものとみなすことが妥当であると言える。
【００８１】
比較のため、Input Shaperの中で最も基本的なZV Shaperにより数１５３を整形した加
減速パターン（数１５４）を図３０に示す。なお，ZV Shaperのパラメータとして、同定した１次モードのモーダルパラメータの平均値を数１５５、数１５６のように用いた。
【数１５３】
【数１５４】
【数１５５】
【数１５６】
つまり、この加減速パターンは１次モードの固有振動数が6.9 Hz，減衰比が0.037である線形時不変系の残留振動を抑制する入力に相当する。時変系である２関節型ロボットアームに適用しても、ある程度残留振動が抑制されることが期待されることから、本発明の手法の比較対象として以下のように取り上げる。
【００８２】
ここで、動作を開始してから最後に指令値を送るまでの時間を動作時間とし、各加減速パターンにより生成される指令値の動作時間を比較する。参照指令値ではｔ＝0.834sで動作終了するのに対し、整形後の指令値ではｔ＝0.915sで動作終了する。この参照指令値に対する動作終了時刻の遅れは、動作終了時の１次モードの半周期π/ω1E＝0.081sに相当する。ここで、ωiEは動作終了時のｉ次モードの固有振動数である。本発明の手法では、一般に参照指令値の動作時間に対して１次固有周期の半分π/ω1Eだけ動作が遅れる。これは、モードごとに２パルスで入力値整形を行うため、最も固有周期の長い１次モードにおいて入力が終わるまでにかかる時間が動作時間となるからである。そのため、高次のモードを扱う場合でも１次固有周期の半分だけ動作が遅れることになる。これに対し、Ｃｈｏの方法などの２パルスのConvolved型のInput Shaperでは、一般に、残留振動を抑制したいモードの固有周期の合計の半分（数１５７）だけ動作終了時刻が遅れる。
【数１５７】
そのため、本発明の手法の方が動作終了時刻を短縮できることが分かる。なお、ZV Shaperでは平均的な固有振動数を用いて加減速パターンを整形するため、動作終了時刻はπ/ωZVだけ遅れる。すなわち、この突き出し動作の場合はｔ＝0.907sで動作が終了する。
【００８３】
（結果と考察）
生成した指令値θshapedを与えた場合に、参照指令値θrefを与えた場合やZV Shaperにより生成した指令値θZVを与えた場合と比べて残留振動がより抑制されることを数値計算と実験により検証した。なお、以下に示す数値計算による結果は、数４をDormand-Prince法により解いて得られるθから計算したものである。
【００８４】
まず、動作終了後の残留振動を評価するため、数値計算により求めた動作終了後のyEを図３１に示し、実験においてレーザー変位計により測定したyEを図３２に示す。図３１、図３２より、実験結果が数値計算の結果をよく再現していることが分かるとともに、本発明の手法がZV Shaperと比べてより効果的に残留振動を抑制していることが分かる。ここで、動作開始から振動振幅が0.25mm以下に収まるまでの時間を位置決め時間とすると、実験結果より、θrefを与えた場合には位置決め時間が2.5秒以上、θZVを与えた場合には1.61秒となった。それらに対し、θshapedを与えた場合には、動作終了時には振動振幅が0.25mm以下となっており、位置決め時間は0.92秒となった。これより、本発明の手法を用いると位置決め時間が短縮され、作業効率を向上させられることが分かる。
【００８５】
次に、動作中の振動を評価するため、数値計算により求めた動作中と動作終了後のxHとyHを図３３に示す。図３３より、θshapedを与えると動作中の振動も抑制されることが分かる。これは、時変性を考慮して入力値整形をしているためである。それに対し、θZVを与えた場合には、動作中である0.3〜0.58秒付近において偶然にも振動が収まっているが、時変性を考慮せずに入力値整形をしているため、動作中のその他の期間と動作終了後において振動が生じる結果となっている。
【００８６】
最後に、数値計算の結果より、座標系ΣBにおいてアーム先端が描く軌跡を図３４に示す。θshapedを与えたときのアーム先端の軌道は、必ずしも参照指令値θrefを与えたときと同じ軌道を描くとは限らないことが図３４より分かる。そのため、アーム先端の軌道に制約がある場合などには、参照指令値θrefを変えて加減速パターンを生成しなおす必要がある。この突き出し動作の例の場合には、アーム先端がyB＞０の領域に大きく侵入していることが分かる。これは、図２５、図２９に示すように、第２関節の参照加速度（数１５８）に比べて、生成した第２関節の加速度パターン（数１５９）では動作初期において大きく加速し、第１関節よりも速く動作するためである。このため、例えばyH＝0.01mの位置に障害物があるような場合には、参照指令値の第２関節の角速度を落とすなどしてから加減速パターンを生成しなおす必要がある。
【数１５８】
【数１５９】
【００８７】
（動作時間と残留振動）
非減衰１自由度線形時不変系において、固有周期の整数倍の時間だけ一定外力を与えると残留振動が生じないということから、図２５のような一定加速度により加減速する加減速パターンを用いても、加速時間と減速時間を調整することにより残留振動をある程度まで抑えられることが予想される。同様に、時変性を考慮しないZV Shaperを用いても加減速時間と系の固有周期の関係により残留振動が効果的に抑えられる場合があると考えられる。しかし、これらの方法では系の固有周期により動作時間を決めなければならないため、必ずしも位置決め時間を短縮することができるとは限らない。それに対し、本発明の手法により生成する加減速パターンは、時変性を考慮しているため、系の固有周期と加減速時間の関係によらず残留振動を抑制でき、位置決め時間を短縮できると考えられる。このことを示すため、動作時間と発生する残留振動の大きさの関係を数値計算と実験により調べた。
【００８８】
まず、一定加速度により加減速する動作について考える。θdを数１６０とする。
【数１６０】
参照指令値として最大角速度をvmax，最大角加速度をＡmaxとし、２つの関節の動作が同時に終了する最短時間制御を行う。θd＜v2max/Ａmaxの場合には図３５のような三角形速度パターンとなり、θd≧v2max/Ａmaxの場合には図３６のような台形速度パターンとなる。このとき加速時間(または減速時間)ｔa、動作時間ｔmは数１６１、数１６２のようになる。
【数１６１】
【数１６２】
【００８９】
ここで、vmax＝πrad/sとし、最大角加速度Ａmaxを与えると、ｔa，ｔm，および数１６３が決まる。この加減速パターンに対応する参照指令値θrefを与えたとき、数値計算により得られる動作終了後(ｔ>ｔm)のアーム先端の最大変位max[|yE|]を図３７に示す。ここで、横軸は動作終了時の１次固有周期に対する加速時間の比（数１６４）である。
【数１６３】
【数１６４】
図３７より、加速時間が１次固有周期の整数倍に近いと発生する残留振動が小さくなることが分かる。しかし、２関節ロボットアームでは非線形性や時変性により、加速時間が１次固有周期の整数倍であっても少なからず残留振動が発生することが分かる。
【００９０】
次に、数値計算により得られる動作終了後のアーム先端の最大変位max[|yE|]を、横軸を動作時間ｔmとして図３８に示す。また、ZV Shaperを適用した場合の最大変位と、本発明の手法を適用した場合の最大変位も示す。なお、最高角速度vmaxをπrad/sとしているので、参照指令値の最高角加速度を大きくして最も制御時間が短くなるようにしても、一定加速度の加減速パターンでは、数１６５だけ動作に時間がかかる。同様にして、ZV Shaperにより整形した加減速パターンでは、数１６６だけ動作に時間がかかり、本発明の手法により生成された加減速パターンでは数１６７だけ動作に時間がかかる。
【数１６５】
【数１６６】
【数１６７】
ここで、実験においてレーザー変位計により測定した最大変位を図３９に示す。図３８、図３９より、動作時間が短い場合には本発明の手法がZV Shaperと比較して動作時間によらず効果的に残留振動を抑制できることが分かる。図３９より、動作時間によっては本発明の手法よりも発生する残留振動の大きさがZV Shaperを用いた方が小さくなる場合があることが分かる。しかし、アーム先端の負荷が変わるなどして固有周期が変化すれば、残留振動が小さくなる動作時間は変化する。そのため、残留振動が生じない動作時間を見つけるのは困難である上、動作時間が限られてしまうため、実際にZV Shaperを多関節型ロボットアームに適用して任意の動作の残留振動を抑制するのは難しい。これに対し、本発明の手法で加減速パターンを生成すると、残留振動を抑制しながら動作時間を短縮できることが分かる。
【００９１】
なお、本発明の手法において、動作時間が短い場合、すなわち、角加速度が大きい場合には、角加速度が小さい場合と比べて残留振動の抑制効果が小さいことが図３８より分かる。本発明の手法も含む各種Input Shaping法では、完全に打ち消し合わせられない振動的な応答が残る場合に、加振力に相当する角加速度が大きいことにより残留振動が大きくなる。時変性を考慮したにも関わらず本発明の手法により振動的な応答が残るのは、加減速パターンの整形前後においてモード形状の時間推移が比較的大きく変わったためだと考えられる。Ａmax＝61.8 rad/s2のときの参照指令値の加減速パターンとそれに対応する速度パターン、指令値を図４０に示す。また、この参照指令値をもとに本発明の手法で生成した加減速パターンとそれに対応する速度パターン、指令値を図４１に示す。両者の速度パターンを比較すると、特に減速期間において速度パターンが異なることが分かる。しかし、加減速パターンを生成する際には加減速パターンの整形前後においてモード形状の時間推移が変わらないことを前提としているため、この前提をもとに生成した加減速パターンでは残留振動が完全に抑制できないということになる。
【００９２】
ここで、図３８と図３９を比較することにより、動作時間が長い場合、すなわち、角加速度が小さい場合には、数値計算による結果と比べて実験ではあまり残留振動が抑制されていないことが分かる。これは、指令角の量子化の精度が粗いためである。動作終了前後における関節１に対する理想的な指令角θshaped1と、実際にサーボドライバに送信されるデジタル化された指令角を図４２に示す。図４２（ａ）には角速度が大きい場合の指令角を示し、図４２（ｂ）には角速度が小さい場合の指令角を示している。図４２より、角加速度が小さい場合には、動作終了直前に非常にゆっくりと回転するように指令をするが、量子化の精度が粗いために、断続的に角度を指令することになる。このため、動作時間が長い場合に残留振動が発生してしまっていると考えられる。このことから、本発明の手法を実際にロボットアームに適用する場合には量子化の精度に注意することが望ましい。
【００９３】
（振り回し動作）
図４３に示すアームを振り回す動作により、ZV Shaperとの違いを明確にする。この動作は、図２４に示す突き出し動作において第２関節２２の回転方向を逆向きにしたものである。つまり、第２関節２２への指令角θ02の符号を反転させただけであり、固有振動数の時間推移は等しくなる。しかし、１次モードに寄与するトルクが大きくなるため、その分だけ残留振動が大きくなり、時変性を考慮しない従来法との違いが顕著になる。
【００９４】
ここで、最大角加速度を4πrad/s2，最大角速度をπrad/sとし，２つの関節が同時に動作終了するような最短時間制御を参照指令値θrefとする。これをもとに、ZV ShaperでθZVを、本発明の手法でθshapedを生成し、数値計算と実験により残留振動の大きさを調べた。yEについて、数値計算の結果を図４４に、実験結果を図４５に示す。これより、時変性を考慮しないZV Shaperと比べて、本発明の手法の方がより効果的に残留振動を抑制できることが分かる。
【００９５】
最大角速度をπrad/s，最大角加速度をＡmaxとし、２つの関節が同時に動作終了する最短時間制御を参照指令値θrefとする。Ａmaxを変化させて、この参照指令値をもとにZV Shaperで生成した指令値θZVと、本発明の手法で生成した指令値θshapedを与えたとき、残留振動のアーム先端の最大変位を数値計算により調べた結果を図４６に示し、実験により調べた結果を図４７に示す。突き出し動作と比べると、１次モードに寄与するトルクが大きいために、実験では残留振動の振幅が全体的に大きくなってしまっている。しかし、時変性を考慮しないZV Shaperでは動作時間により残留振動が抑制される場合とそうでない場合があるのに対し、本発明の手法では動作時間によらず残留振動が抑制されている。このことから、本発明の手法が多関節型ロボットアームに有効であると言える。
【００９６】
（折りたたみ動作）
上述の説明にて述べたとおり、本発明の手法では全てのモードの残留振動を抑制する。しかしながら、これまでに見てきた動作例では２次の振動モードがほとんど励起されなかった。そこで、２次モードの振動も抑制されることを示すため、図４８に示すアームを折りたたむ動作により発生する残留振動を、実験によりレーザー変位計で測定した。測定した座標系ΣEにおけるアーム先端の位置を図４９に示す。図４９よりyE方向の変位には２次モードの成分が多く含まれていると考えられる。そこで、yEのパワースペクトルを図５０に示す。図５０より、２次モードについてもピークレベルが下がっており、本発明の手法により複数の振動モードが抑制されることが分かる。
【００９７】
なお、図５０より、残留振動を抑制し、偏差角が小さくなると、固有振動数が変化することが分かる。これは、波動歯車装置の非線形剛性や、サーボ機構によるものだと考えられる。本発明の手法では、このような剛性の非線形性を厳密に考慮することはできない。しかしながら、上述にて説明した３つの動作例による実験結果から、剛性が線形であるとしてアクセレランスなどをもとにパラメータ同定をすることである程度まで振動が抑えることができることが分かる。
【００９８】
（実施例３：垂直２関節型ロボットアーム）
次に、実施例３として、垂直２関節型ロボットアームへの適用した例について説明する。具体的には、図５１に示す鉛直面内で動作する２関節ロボットアーム２０について、残留振動を抑制するための加減速パターンを生成し、数値計算により重力がある場合でも本発明の手法が有効であることを検証した。
【００９９】
（運動方程式）
関節ｊの回転角度をθj，その指令値をθ0j，リンクｌの質量をｍｌ，リンクｌの質量中心まわりの慣性モーメントをＪGｌ，リンクｌの長さをｂｌ，関節ｊからリンクｌの質量中心までの距離をｌj，関節ｊのねじり剛性をｋjとする。運動方程式は数１６８のようになる。ただし、数１６９〜数１７９の関係にある。
【数１６８】
【数１６９】
【数１７０】
【数１７１】
【数１７２】
【数１７３】
【数１７４】
【数１７５】
【数１７６】
【数１７７】
【数１７８】
【数１７９】
また、ｇ＝9.8m/s2は重力加速度であり、図５１に示す方向に重力が働いているものとする。数５〜数１０によるテイラー展開により、参照指令値をθref＝[θref1 θref2]Tとすると、垂直２関節型ロボットアームの運動方程式は数２１で表される。ただし、各パラメータは数１１〜数１５より、数１８０〜数１８３のようになる。
【数１８０】
【数１８１】
【数１８２】
【数１８３】
なお、数１８４〜数１９０の関係にある。
【数１８４】
【数１８５】
【数１８６】
【数１８７】
【数１８８】
【数１８９】
【数１９０】
【数１９１】
【数１９２】
ここで、減衰力は各関節の偏差角の変化率に比例して働くものとすると、粘性減衰行列Ｃvは、数１９３のように表される。なお、本実施例３では、図５２の表に示す諸元を用いて以下の数値計算を行った。
【数１９３】
【０１００】
（突き出し動作での加減速パターンの生成）
図２４に示す突き出し動作について考える。重力による釣り合いの位置を考慮すれば動作開始時の指令値θ0S＝ [θ01S θ02S]Tは、数１９４を満たすθ0Sとなる。ここで数１９５である。
【数１９４】
【数１９５】
同様にして、動作終了時の指令角θ0E＝[θ01E θ02E]Tは、数１９６を満たすθ0Eとなる。
【数１９６】
図２４のように、θ1S＝7π/24＝0.916rad，θ2S＝−7π/12＝−1.83rad，θ1E＝θ2E＝0radである場合には。θ01S＝0.926rad，θ02S＝−1.83rad，θ01E＝0.016rad，θ02E＝0.004radとなる。このことを考慮し、最大角加速度を4πrad/s2，最大角速度をπrad/sとすると、最短時間制御は図５３に示すような指令値となる。これを参照指令値θrefとし、対応する加減速パターン（数１９７），速度パターン（数１９８）についても図５３に示す。
【数１９７】
【数１９８】
【０１０１】
Ｋ-1(t)ｇref(t)の時間微分を解析的に求めるのは困難であるので、数１５２と同様に数１９９、数２００と定義する。
【数１９９】
【数２００】
数１９９、数２００、および数１５２を用いて、数６４により得られる加速度パターン（数５９）と対応する（数６０）、（数６１）を図５４に示す。ただし、ΔtP＝0.001s，ΔtG＝10-5sとした。整形前の図５３と整形後の図５４とを比べることにより、両者の速度パターンと角度指令値は大きく違わないことが分かる。これにより、モード形状の時間推移は整形後も変わらないものとみなすことが妥当であると言える。
【０１０２】
（結果と考察）
参照指令値θrefを与えた場合と比べて、整形した加減速パターン（数５９）に対応する角度指令値θshapedを与えたときの方が、残留振動が抑制されることを数値計算により示した。θrefを与えた場合とθshapedを与えた場合について、それぞれDormand-Prince法により数４を数値的に解いた。解いた結果得られるθから計算した、動作終了後のアーム先端の座標系ΣBにおけるy方向の変位を図５５に示す。これより、本発明の手法が、重力を考慮しなければならない垂直２関節ロボットアームの残留振動抑制にも有効であることが分かる。
【０１０３】
また、数値計算の結果より、動作中の各関節の角速度θを図５６に示し、釣り合いの位置からの偏差角ψ＝[ψ1 ψ2]Tを図５７に示す。これらより、水平２関節型ロボットアームの場合と同様に、動作中の振動が抑制されているのが分かる。
【０１０４】
これらの数値計算の結果より、釣り合いの位置からの偏差角ψを数１８と定義し、ψに関する運動方程式を数２１のように書くことで、水平２関節型ロボットアームと同じように残留振動を抑制する加減速パターンを生成できることが分かる。
【０１０５】
本発明によれば、加速または減速運動に伴って運動体に生じるｎ自由度かつ時変の振動を、インプットシェイピング法を用いて抑制する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第２の入力を決定するという手法を採用している。
【０１０６】
また、本発明では、ｎ自由度かつ時変の振動におけるモード間の相互作用によるエネルギの移り変わりをゼロと見なして、固有振動数および減衰比の時間変化によるモード間のエネルギの移り変わりを考慮して、第１の入力に対する第２の入力の大きさを決定するという手法を採用していると言うこともできる。
【０１０７】
また、別の観点からは、本発明では、ｎ自由度かつ時変の振動における複数のモード間の相互作用によるエネルギの移り変わりを無視し、加速または減速運動を行っている間に運動体自体の姿勢が変化することにより生じるコリオリがした仕事を考慮して、第１の入力に対する第２の入力の大きさを決定するという手法を採用していると言うこともできる。
【０１０８】
このような手法を採用していることにより、各振動モードにおいて、第１の入力に対応する入力を打ち消す第２の入力に対応する入力の大きさを個別に算出して、振動モードごとに算出された入力を合成することで第２の入力の大きさを決定することが可能となる。
【０１０９】
そのため、本発明によれば、第１の入力によって生じたｎ自由度かつ時変の振動（第１の応答）を抑制できる第２の入力を算出することが可能となる。
【０１１０】
よって、ロボットアーム、工作機械、およびクレーンなど、その動作中に姿勢変化を伴うような動作を行う運動体に生じる振動、すなわち、ｎ自由度かつ時変の振動を、インプットシェイピング法を適用して効果的に抑制する振動抑制方法を提供することができる。
【０１１１】
なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。
【符号の説明】
【０１１２】
１０ ロボットアーム
２０ ロボットアーム
Ｊｎ 関節
Ｌｎ リンク
Ｐ１ 第１のインパルス
Ｐ２ 第２のインパルス
Ｐ３ 第１のインパルス列
Ｐ４ 第２のインパルス列
【技術分野】
【０００１】
本発明は、加速または減速運動に伴って運動体に生じる振動を抑制する振動抑制方法に関するものであり、特に、運動中に運動体自体の姿勢が変化する場合に、運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法に関する。
【背景技術】
【０００２】
様々な作業を行うために運動動作を行うロボットアームは、モータとアームの間に減速機を用いている。減速機はねじりばねとして振る舞うため、モータ停止後もアームの振動が残留する。その結果、振動が収まるまで次の作業に移ることができず、作業効率を向上させることができない。
【０００３】
このような残留振動に伴う課題は、ロボットアームだけに限られず、加速または減速運動を含む運動動作を行う運動体、例えば、工作機械やクレーン等についても共通する。
【０００４】
フィードバック制御で振動を抑える場合は検出器が必要となるため、検出器が不要なフィードフォワード制御で振動を抑える方法が求められている。
【０００５】
フィードフォワード制御での振動抑制方法としては、インプットシェイピング法が知られている（例えば、非特許文献１参照）。このインプットシェイピング法は、工作機械などのように、固有振動数が変化しない場合に有効な振動抑制方法であり、様々な提案がなされている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−２９６１７号公報
【非特許文献】
【０００７】
【非特許文献１】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
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ロボットアームは、様々な作業に対応可能とするために、一般に多関節および多自由度を有している。そのため、ロボットアームにおいて、一の関節が駆動された運動動作を行っている間に他の関節が駆動されると、運動動作を行っているアーム自体の姿勢が変化する。そのため、アームの固有振動数が時間によって変化することになる。このような場合にあっては時変の振動系となるため、時不変の振動系を対象として従来のインプットシェイピング法を単に適用するだけでは、振動抑制に対応することができない。
【０００９】
このような時変の振動系において振動を抑制する方法としては、いくつかの提案がなされている（例えば、特許文献１および非特許文献２参照）。しかしながら、時変の振動抑制に対して、インプットシェイピング法を効果的に適用できる振動抑制方法が求められている。特に、自由度が２以上のロボットアームに対しては、複数の固有振動数が時々刻々と変化していくため、インプットシェイピング法を用いて多自由度かつ時変の振動を効果的に抑制することが求められている。
【００１０】
従って、本発明の目的は、上記課題を解決することにあって、加速または減速運動に伴って運動体に生じるｎ自由度かつ時変の振動を、インプットシェイピング法を効果的に適用して抑制する振動抑制方法を提供することにある。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、本発明は以下のように構成する。
【００１２】
本発明の第１態様によれば、加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法を提供する。
【００１３】
本発明の第２態様によれば、加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間の相互作用によるエネルギの移り変わりを無視し、加速または減速運動を行っている間に運動体自体の姿勢が変化することにより生じるコリオリがした仕事を考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法を提供する。
【００１４】
本発明の第３態様によれば、加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動におけるモード間の相互作用によるエネルギの移り変わりをゼロと見なして、固有振動数および減衰比の時間変化によるモード間のエネルギの移り変わりを考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法を提供する。
【００１５】
本発明の第４態様によれば、各振動モードにおいて、第１の入力に対応する入力を打ち消す第２の入力に対応する入力の大きさを個別に算出して、振動モードごとに算出された入力を合成することで第２の入力の大きさを決定する、第１態様から第３態様のいずれか１つに記載の振動抑制方法を提供する。
【００１６】
本発明の第５態様によれば、運動体が多関節型ロボットアームであって、ロボットアームに生じるｎ自由度かつ時変の振動を抑制する、第１態様から第４態様のいずれか１つに記載の振動抑制方法を提供する。
【発明の効果】
【００１７】
本発明によれば、加速または減速運動に伴って運動体に生じる、ｎ自由度かつ時変の振動を、インプットシェイピング法を用いて抑制する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第２の入力を決定するという手法を採用している。そのため、第１の入力によって生じたｎ自由度かつ時変の振動（第１の応答）を抑制できる第２の入力を算出することが可能となる。よって、加速または減速運動に伴って運動体に生じるｎ自由度かつ時変の振動を、インプットシェイピング法適用して効果的に抑制する振動抑制方法を提供することができる。
【図面の簡単な説明】
【００１８】
【図１】本発明の実施の形態にかかる振動抑制方法が適用される多関節型ロボットアームの模式図
【図２】図１のロボットアームにおける関節の模式図
【図３】時変におけるインパルス応答の変位と時間の関係図
【図４】時変におけるインパルスの重ね合わせの考え方を示す図
【図５】２関節ロボットアームにおける各モードの振動形態を示す図
【図６】実施例１にかかる可変ばねを備える１自由度線形時変系の構成図
【図７】インパルス応答Ｈ(t,0)を示す図
【図８】追加入力と時間の関係図
【図９】オリジナル系の応答と近似式による応答との比較図（ａ＝π）
【図１０】参照加速度指令値と追加指令値との波形図
【図１１】整形した加速度指令値の波形図
【図１２】加速度指令値に対する応答の波形図
【図１３】整形された加速度指令値とＣｈｏ方法による加速度指令値の波形図
【図１４】残留振動の低減効果を示す図
【図１５】実施例２にかかる水平２関節型ロボットアームの構成図
【図１６】水平２関節型ロボットアームの実験モデルの上面図
【図１７】サーボドライバの設定値を示す表
【図１８】実験モデルの測定パラメータを示す表
【図１９】自由応答の測定方法の模式図
【図２０】２関節型ロボットアームのアクセラレンスを示す図
【図２１】実験モデルのパラメータを示す表
【図２２】実固有値解析により求まる固有振動数を示す図
【図２３】ロボットアームにおける座標系の定義を示す図
【図２４】２関節型ロボットアームの突き出し動作の模式図
【図２５】突き出し動作における指令値を示す図
【図２６】突き出し動作中の疑似固有振動数と疑似減衰比を示す図
【図２７】オリジナル系と近似式による応答との比較図
【図２８】近似における応答誤差を示す図
【図２９】整形後の加減速パターンを示す図
【図３０】ZV Shaperにより整形した加減速パターンを示す図
【図３１】突き出し動作におけるΣE座標系のアーム先端の変位を示す図（計算値）
【図３２】突き出し動作におけるΣE座標系のアーム先端の変位を示す図（実験値）
【図３３】突き出し動作におけるΣH座標系のアーム先端の変位を示す図（計算値）
【図３４】突き出し動作におけるΣB座標系のアーム先端の変位を示す図（計算値）
【図３５】三角形速度パターンの図
【図３６】台形速度パターンの図
【図３７】アーム先端の最大変位を示す図
【図３８】アーム先端の最大変位を示す図（計算値）
【図３９】アーム先端の最大変位を示す図（実験値）
【図４０】加減速パターン、速度パターン、指令値を示す図
【図４１】整形した加減速パターン、速度パターン、指令値を示す図
【図４２】理想指令角と、送信されるデジタル化された指令角を示す図
【図４３】２関節型ロボットアームの振り回し動作の模式図
【図４４】振り回し動作後のΣE座標系におけるアーム先端の変位を示す図（計算値）
【図４５】振り回し動作後のΣE座標系におけるアーム先端の変位を示す図（実験値）
【図４６】アーム先端の最大変位を示す図（計算値）
【図４７】アーム先端の最大変位を示す図（実験値）
【図４８】２関節型ロボットアームの折り畳み動作の模式図
【図４９】折り畳み動作後のΣE座標系におけるアーム先端の変位を示す図（実験値）
【図５０】残留振動のパワースペクトル図
【図５１】実施例３にかかる垂直２関節型ロボットアームの構成図
【図５２】垂直２関節型ロボットアームのパラメータを示す表
【図５３】参照指令値、加減速パターン、速度パターンを示す図
【図５４】整形後の参照指令値、加減速パターン、速度パターンを示す図
【図５５】突き出し動作後のΣB座標系におけるアーム先端の変位を示す図
【図５６】動作中の各関節の角速度を示す図
【図５７】釣り合い位置からの偏差角を示す図
【発明を実施するための形態】
【００１９】
以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。
【００２０】
（実施の形態）
本発明の実施の形態にかかる運動体の振動抑制方法について、運動体の一例であるロボットアーム１０を用いて説明する。
【００２１】
（多関節型ロボットアームの運動方程式）
図１に示すように、ロボットアーム１０は、多関節型ロボットアームであり、ｎ個のリンクＬ１，・・・，Ｌｎと、それぞれのリンクＬを動作可能に接続するｎ個の関節Ｊ１，・・・，Ｊｎとを備えている（ｎは自然数）。また、関節Ｊの部分を示す図２および図１に示すように、このロボットアーム１０のそれぞれのリンクＬおよび関節Ｊは、１つの平面内で動作可能となっている。
【００２２】
図２に示すように、関節Ｊ１の角度をθ1、その指令角をθ01、ばね定数をｋ1とし、関節Ｊ１以外の任意の関節ｊの角度をθj、その指令角をθ0j、ばね定数をｋjとする。上付き添え字Ｔは転置を表すとし、数１〜数３のように定義する。
【数１】
【数２】
【数３】
【００２３】
数１〜数３の定義と、ラグランジュの方法により、多関節型ロボットアーム１０の運動方程式は、数４のように表される。
【数４】
ここで、Ｊ（θ）は慣性行列、ｈ（左辺第２項）はコリオリの力と遠心力を表す項、ｇ（θ）は重力付加を表す項、ＣVは粘性減衰行列である。
【００２４】
（テイラー展開）
次に、非線形な式である数４をθ＝θ0まわりの１次のテイラー展開により線形化を行うと、数５が得られる。なお、数５におけるφ、Ｃh、Ｋm、Ｋh、Ｋgは、数６〜数１０の通りである。
【数５】
【数６】
【数７】
【数８】
【数９】
【数１０】
【００２５】
（多自由度線形時変系のモード分解）
モード解析は、本来は線形時不変系を対象とする解析手法の一つである。ここでは、このモード分解を連続時間で記述される線形時変系に適用し、入力値整形に利用する。
【００２６】
一般にｎ自由度線形時変系の運動方程式は、ｎ次正方実行列の質量行列Ｍ(t)、減衰
行列Ｃ(t)、剛性行列Ｋ(t)、ｎ行縦ベクトルの入力ｆ(t)、偏差角ベクトルφ(t)を用いて、数１１のように表せる。
【数１１】
【００２７】
多関節型ロボットアーム１０の場合には、指令値θ0に対して参照指令値θref(t)を与えるものとして、数１２〜数１５とすれば、数５は数１１の形の多自由度線形時変系とみなすことができる。ただし、ｈref(t)、ｇref(t)は数１６、数１７のように表される。
【数１２】
【数１３】
【数１４】
【数１５】
【数１６】
【数１７】
【００２８】
ここで釣り合いの位置からの偏差角ψは数１８の通りである。
【数１８】
この時間に関する１階微分と２階微分はそれぞれ数１９、数２０のようであるので、釣り合いの位置からの偏差角ψに関する運動方程式は数２１のようになり、数２２のように表される。
【数１９】
【数２０】
【数２１】
【数２２】
【００２９】
Ｅｎをｎ次単位行列とすると、数２１の運動方程式に対応する状態方程式は、数２３〜数２６にように表される。
【数２３】
【数２４】
【数２５】
【数２６】
【００３０】
ここで、Ａの時刻ｔにおける瞬間的な固有ベクトルと固有値について、常に共役なペアがｎ個存在する場合、つまり、全モードの応答が振動的であり、過減衰または臨界減衰でない場合についてのみ考える。Ａの時刻ｔにおけるｉ番目の瞬間的な固有ベクトルをｐi(t)、対応する瞬間的な固有値をλi(t)とし、固有ベクトルからなる行列Ｐ(t)と固有値を対角要素にもつ行列Λ(t)を、数２７、数２８のように定義する（ｉは自然数）。ただし、数２９の関係式が成立するようにｉを選ぶものとする。
【数２７】
【数２８】
【数２９】
【００３１】
ここで、モード座標ｑを数３０のように定義する。すると、状態方程式はモード座標について、数３１のように表すことができる。
【数３０】
【数３１】
ここで、数３１に示される状態方程式における右辺第一項である数３２にて表される行列のうち対角要素のみを取り出した行列Ｄ(t)を考え、この行列Ｄ(t)を数３３のように表す。
【数３２】
【数３３】
数３２の行列において、非対角要素の絶対値が対角要素の絶対値に比べて十分に小さいことから、数３３の行列Ｄ(t)を用いて、数３１の状態方程式を数３４のように近似できる。なお、この状態方程式の近似の考え方については後述する。
【数３４】
【００３２】
数３１の状態方程式では、数３２の行列において、非対角要素に有意な値があるため、このままでは方程式を解くことができない。しかしながら、数３１の状態方程式を数３４の状態方程式にように近似することで、各モードの応答を独立な方程式を解くことで求めることが可能となる。これにより、ｉ次モードのインパルス応答関数を数３５のように導くことができる。なお、τは単位インパルスを与える時刻である。
【数３５】
【００３３】
（モードごとの入力値整形）
次に、ロボットアーム１０に付加された第１の入力によってロボットアーム１０に生じる第１の応答に、ロボットアーム１０に第２の入力を付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うような第２の入力の値を、振動モードごとに算出方法について説明する。
【００３４】
ここで、インプットシェイピング法におけるインパルス応答の重ね合わせの考え方について、図３に示す。図３ではインパルス応答の変位（振動量：縦軸）と時間（横軸）との関係を示している。
【００３５】
図３に示すように、ある時刻τに与えられる第１のインパルスＰ１（第１の入力）に対する第１の応答Ｈi(ｔ,τ)（第１の応答）を打ち消すため、第２のインパルスＰ２（第２の入力）を与える時刻をτ~（数式上では、「τ」の上に「~」と表す。以降同じ。）（＞τ）とし、第１のインパルスＰ１に対する第２のインパルスＰ２の大きさの比を実数α~（数式上では、「α」の上に「~」と表す。以降同じ。）とする。ここで、α~とτ~は未知数であり、これらは以下のように求めることができる。
【００３６】
まず、第１のインパルスＰ１により生じる第１の応答とインパルスＰ２により生じる第２の応答とが、ｔ＞τ~において互いに打ち消しあうための条件は、数３６にて表される。
【数３６】
数３５より、ｔ＞τ~では、数３７の関係式が成り立つため、数３６は数３８のようになる。
【数３７】
【数３８】
一般にＨi(ｔ,τ~)≠０であるので、数３６を満たすための条件は、数３９のようになる。
【数３９】
この数３９は、第２のインパルスＰ２の大きさを求める式である。τ~は未知であり、α~は実数であるので、数４０に示す関係を満たさなければならない。
【数４０】
数４０を用いて、時刻τで与えられた第１のインパルスＰ１に対する第１の応答の虚部が０になる時刻τ~を求めることができる。しかし、数４０を満たすτ~は無数に存在する。そこで、数４０を満たす最小のτ~をτ'とする。なお、τ'は力学系においては、第１のインパルスＰ１によって生じる第１の応答に対して、変位が最初に０になる時刻である。また、(τ'−τ)は、時不変系で言うところの半周期に相当する。時変系であれば、τ'は第１のインパルスＰ１を与える時刻τにより異なるので、τ'をτの実関数として、数４１にように表す。
【数４１】
さらに、αi(τ)を数４２のように定義すれば、数４３の関係式が成り立つ。
【数４２】
【数４３】
【００３７】
次に、このインパルスの組み合わせを用いて、ｉ次モードに対する任意の入力ｕi(ｔ)による応答を打ち消すことができる追加の入力ｗi(ｔ)を求める。任意の入力ｕi(ｔ)はある時刻τSからτEの間にのみ任意の複素数をとるものとし、それ以外の時刻(ｔ＜τS，ｔ＞τE）では０とする。数４３にｕi(ｔ)を乗じ、τについてτSからτEまで積分すると、τ＞ηi（τE）のとき、数４４が成り立つ。
【数４４】
数４４の左辺第一項は畳み込み積分の形になっており、入力ｕi(ｔ)に対する応答を表している。左辺第二項についても畳み込み積分の形にするため、積分変数をτからτ'に変換する。なお、ηiの逆関数をηi-1、ηi-1の１次導関数を数４５のように表すと、数４６、数４７のようになる。
【数４５】
【数４６】
【数４７】
これらより、数４４は数４８のように表される。ただし、数４９〜数５１のような関係になる。
【数４８】
【数４９】
【数５０】
【数５１】
【００３８】
数４８の左辺第一項は、ｕi(ｔ)による応答、左辺第二項はｗi(ｔ)による応答を表すことになる。さらに、これらの足し合わせがｔ＞τ'Eにおいて０になることを、数４８は示している。つまり、ｕi(ｔ)＋ｗi(ｔ)をｉ次モードに対する第２の入力とすれば、このｉ次モードに生じた残留振動は抑えられることになる。なお、第１のインパルスＰ１が与えられた時刻τから第１の応答の虚部が最初に０になるまでの時間（τ'−τ）を時変系における半周期と呼ぶことにすれば、数５１においてηi-1(τ')は、時刻τ'から見て半周期前の時刻τを表すことになる。また、αi(ηi-1(τ'))は時刻τに与えられた第１のインパルスＰ１に対する第１の応答が、半周期の間にどれだけ減衰するのかを表しており、ｕi(ηi-1(τ'))は半周期前の入力の大きさを表すことになる。また、数５２は、時変系における固有周期の変化に対する補正量である。
【数５２】
ここで、時変系において連続的に第１のインパルスが入力される場合を考える。図４に示すように、第１の入力として、連続的にインパルスが入力された第１のインパルス列Ｐ３により生じる第１の応答を打ち消すように連続的にインパルスが入力された第２のインパルス列Ｐ４を考える。時変系の場合は、第１のインパルス列Ｐ３の時間間隔がｄτであるのに対して、第２のインパルス列Ｐ４の時間間隔は数４８における積分変数の変換により数５３のように表される。そのため、この時間間隔を補正するために追加の入力に対しては、数５２に示される時変系における固有周期の変化に対する補正量を乗じる必要がある。
【数５３】
【００３９】
ここで、元々の入力ｕi(ｔ)に対してｗi(ｔ)を足し合わせると、例えばこの入力を力とみれば、系に加わる力積が単純に増えるということになる。そこで、入力の正規化係数を数５４で定義する。
【数５４】
数４８に任意の複素定数を乗じても右辺が０となることから、ｕi(ｔ)＋ｗi(ｔ)に任意の複素定数を乗じても２つの入力は打ち消し合うことになる。そこで、数５４で定義される正規化係数を用いて、各モードにおける整形された入力値は、数５５のように表すことができる。
【数５５】
【００４０】
（複数モードを対象とした入力値整形）
次に、複数モードを対象とした入力値整形の方法を示す。まず、入力ｆとして任意の参照入力ｆrefを与えると、対応する各モードへの入力ｕiは、数５６のように表される。
【数５６】
この数５６をもとに数５５により整形された入力ｕ'i(ｔ)をモードごとに求め、任意の実定数κ1,・・・,κnを用いて、数５７のように表す。
【数５７】
Ｂ＋をＢの疑似逆行列とすると、数５８で表されるｆshapedが、数５６で表される多自由度線形時変系において、全てのモードの残留振動を抑制する入力となる。κiｕ'iがｉ次モードの残留振動を抑制する入力であり、さらにその他のモードを励起しないため、そのような入力の足し合わせであるｆshapedは、結局、全てのモードについての残留振動を抑制することになる。
【数５８】
【００４１】
（加減速パターンの生成）
次に、ロボットアーム１０の残留振動を抑制する加減速パターン（数５９）を生成する方法について説明する。本手法により加減速パターンを整形した結果、対応する速度パターン（数６０）、変位指令値（数６１）は、（数６２）、（数６３）から大きく変わらないものとする。これにより、加減速パターンを整形した後もモード形状の時間推移は変わらないものとみなせる。すると、数１５、数２２より、数６４で表される加減速パターン（数５９）が残留振動を抑制する加減速パターンとして生成される。ただし、κ＝[κ1・・・κn]は動作終了時に、対応する速度パターン（数６０）が０となるように、信頼領域Dogleg法によりκを変化させながら数５６〜数５８、数６４を繰り返し評価することにより決めることが望ましい。また、Γは対角実行列であり、整形後の指令値（数６１）が参照指令値（数６３）と同じ値で終わるようにその要素を決めることが望ましい。
【数５９】
【数６０】
【数６１】
【数６２】
【数６３】
【数６４】
【００４２】
（状態方程式の近似について）
ここで、上述した数３１の状態方程式において、数３２にて表される右辺第一項の行列のうちの非対角要素を無視して（すなわち０として）、対角要素のみを取り出した行列Ｄ(t)を用いて、数３４のように近似した本発明の考え方について、さらに説明する。
【００４３】
２関節ロボットアーム２０を例として説明する。図５に示すように、２関節ロボット２０では２種類の振動の形態が考えられる。図５（ａ）に示す振動の形態が１次モードであり、図５（ｂ）に示す形態が２次モードである。このような振動の形態は、１次モードについては固有ベクトルｐ１によって決まり、２次モードについては固有ベクトルｐ２によって決まる。実際には、この２つの振動モードが合成されて（混在して）アームが振動し、２つの振動の形態が合成された動きとなる。
【００４４】
時変系の場合は、数３１において、行列（数３２）の非対角要素が０でないため、ｑ1の値がｑ2に影響を及ぼし、ｑ2の値がｑ1に影響を及ぼすことになる。すなわち、時変系の場合には振動モード間での相互作用が発生することになる。これに対して、時不変系の場合には振動モード間での相互作用は発生しない。時変系におけるこの相互作用の大きさが行列（数３２）の非対角要素の値の大きさと対応する。
【００４５】
しかしながら、運動体がロボットアームの場合には、振動モード間での相互作用はあまり大きくない。具体的には、行列（数３２）の対角要素（モード毎の自らの時変性が示されている要素）に比して、非対角要素（モード間の相互作用の大きさが示されている要素）が十分に小さい。そこで、この非対角要素を無視して０とみなし、数３２の対角要素のみを取り出したＤという行列（数３３）を用いることで、数３１の状態方程式を数３４のように近似することで、モードごとの応答を独立な方程式を解くことで算出することを可能とするというのが、本発明の考え方である。なお、このような観点からは、本発明において「振動モード間の相互作用」とは、数３１において、ｑ1の値がｑ2に影響を及ぼし、ｑ2の値がｑ1に影響を及ぼす作用を意味すると言うことができる。
【００４６】
また、モード間のエネルギのやり取りの観点からも、この相互作用について説明できる。系に減衰が無く、外力も与えられていないとすれば、エネルギ保存則が成り立つ。１次モードのエネルギをＥ1、２次モードのエネルギをＥ2とすれば、Ｅ1＋Ｅ2は常に一定の値となる。時不変系であれば、さらにモードごとにエネルギ保存則が成り立つ。つまり、Ｅ1とＥ2がそれぞれ一定の値となる。これに対して、時変系では、モードごとにエネルギ保存則が成り立たず、モード間でエネルギのやり取りが発生する。例えば、Ｅ1が大きくなるときには、Ｅ2がその分だけ小さくなる。逆に、Ｅ1が小さくなるときには、Ｅ2はその分だけ大きくなる。
【００４７】
ここで、このような時変系におけるモード間でのエネルギのやり取りには２つの要因がある。１つ目は、行列（数３２）の対角要素、つまり固有振動数と減縮比が時間により変化することである。２つ目は、行列（数３２）の非対角要素により異なるモード間にて相互に作用し合うことによるものである。本発明では、時変系において、１つ目の要因を考慮しながら、２つ目の要因を無視することにより、数３４のような近似式を得るという考え方に基づいている。
【００４８】
すなわち、本発明の振動抑制方法では、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第１の入力により生じる第１の応答を打ち消すような第２の入力の大きさを決定する、という考え方に基づいている。
【００４９】
次に、このような考え方に基づく、ｎ自由度かつ時変の振動を抑制する振動抑制方法について、具体的な実施例を用いて説明する。
【００５０】
（実施例１：１自由度線形時変系）
まず、実施例１として、１自由度線形時変系について説明する。具体的には、上記実施の形態にて説明した手法を、図６に示す可変ばねを有する１自由度振動系に適用し、本手法の時変系に対する有効性を確かめる。特に固有周期の変化に対する補正が、高速にパラメータが変化する際に重要となることを本実施例１にて示す。
【００５１】
（運動方程式）
図６に示す可変ばねを有する１自由度振動系を考える。ばねの自然長は一定である。質量は１に正規化し、図示右向きを正として質量の位置をξとする。固有振動数をωn(t)とすると、ばね定数はωn2となる。また、右向きを正として指令値をξ0とする。すると、この系の運動方程式は数６５で表される。
【数６５】
ここで、質量の変位と指令値の偏差ｚ＝ξ−ξ0について運動方程式を書きなおすと、数６６で表される。なお、初期条件は数６７にて表される条件とする。
【数６６】
【数６７】
この偏差に関する運動方程式に対応する状態方程式は、数６８のように表される。
【数６８】
数６８において、Ａ(t)の時刻ｔにおける瞬間的な固有ベクトｐ(t)と固有値λ(t)はそれぞれ数６９、数７０にて表される。
【数６９】
【数７０】
【００５２】
（モード座標への変換）
次に、モード座標への変換を行う。数７１および数７２とすれば、数７３にて表されるモード座標を用いて、状態方程式は数７４と書き換えられる。
【数７１】
【数７２】
【数７３】
【数７４】
ここで、数７５に表されるようにし、数７４の非対角要素を無視すれば、数７４は数７６のように近似できる。
【数７５】
【数７６】
各モードへの入力を数７７に示すようにすると、モードごとの運動方程式は、数７８にて表すことができる。
【数７７】
【数７８】
ここで、数７９とすれば、ｄの時間積分は数８０と書けるので、インパルス応答関数は、数８１と求まる。
【数７９】
【数８０】
【数８１】
【００５３】
（入力値整形）
次に、数４０を満たす数８２の最小値をτ'とすると、数８３となる。
【数８２】
【数８３】
これをτ'とτについて解き、それぞれτ'＝η(τ)，τ＝η−１(τ')とする。また、α(τ)＝−Ｈ(η(τ),τ)とする。数５１より、数８４により励起される振動を打ち消す加速度指令値は、数８５のように求めることができる。
【数８４】
【数８５】
【００５４】
（シミュレーション）
シミュレーションの例として、固有振動数が数８６のようになる場合について考える。ここで、ω0(＞０)はｔ＝０における初期固有振動数、ａ(＞０)は固有振動数の時間変化率を表す実定数である。すると、各種関数が、数８７〜数９１のように、初等関数のみを使って求めることができる。
【数８６】
【数８７】
【数８８】
【数８９】
【数９０】
【数９１】
参考のため、ａ＝π，ω0＝πの場合の数９０を図７に示し、数８８、数８９および数９１を図８に示す。
【００５５】
ここで、初速度０の状態から数９２の状態まで加速するための参照加速度指令値として、数９３を与えるものとする。
【数９２】
【数９３】
非対角要素を無視した数７６の近似の妥当性を調べるため、数９３を入力として与えたとき、数７４による応答ｑと近似式である数７６による応答ｑを、それぞれルンゲクッタ法の一種であるDormand-Prince法により図９のように得た。図９に示されるように、２つの応答はよく似ており、数７６の近似は妥当であると言える。
【００５６】
参照加速度指令値（数９３）が与えられれば、数８５を用いると、初等関数のみを用いて明示的に振動を打ち消す加速度指令値（数８５）を求めることができる。このようにして求めた振動を打ち消す加速度指令値を図１０に示す。ここで、残留振動を抑えつつ、加速期間終了時ｔ＝η(１)に速度（数９４）が１ｍ／ｓとなるように、加速度指令値を数９５とする。
【数９４】
【数９５】
参照加速度指令値（数９６）、整形後の加速度指令値（数９７）、Ｃｈｏの方法により整形した加速度指令値を図１１に示し、それぞれに対する応答を図１２に示す。図１２より明らかなように、本発明の手法で残留振動が大幅に抑制されていることが分かる。なお、Ｃｈｏの方法については参考文献（Cho, J. K. and Park, Y. S., “Vibration reduction in flexible systems using a time-varying impulse sequence”, Robotica, Vol. 13, Issue 3 (1995), pp. 303-313.）に開示されている。
【数９６】
【数９７】
【００５７】
特に着目すべき点は、本発明の手法(Proposed method)では追加の入力に相当する部分がＣｈｏの方法に比べて大きい点である。これは、時間が経つにつれて固有周期が短くなっているためである。このような場合には参照入力と追加の入力が与える力積を等しくするために、追加の入力の方を大きくなるよう補正する必要がある。本発明の手法では、この固有周期の変化に対する補正を数５２で示す項で行っている。一方で、図１３に示すように、Ｃｈｏの方法により得られる加速度指令値は数５２で示す項を１として整形した加速度指令値と一致することから、Ｃｈｏの方法では固有周期の変化に対する補正がされてないと言える。このことから、固有周期の変化に対する補正が時変系の残留振動抑制に必要であることが分かる。
【００５８】
また、数７５の積分が解析的に求まり、さらに、数８３が解析的に解けるのであれば、本発明の手法では単純な演算のみで入力値整形を行うことができる。実際、固有振動数の変化が数８６のように表せるため、初等関数のみを用いて入力値整形を行うことができた。このように、１自由度時変振動系を対象とする場合には、本発明の手法による入力値整形の演算量は少なくて済む。このため、多くの行列演算を必要とするＣｈｏの方法と比べて、本発明の手法は実機に実装するにあたって有用である。
【００５９】
（高速時変系に対する効果）
本発明の手法の特徴は、パラメータが高速に変化する場合にでも適用可能な点にある。先に説明した例を用いて、固有振動数の変化率ａが大きい場合でも残留振動が低減可能であることを数値計算によるシミュレーションにより示す。
【００６０】
まず、数９８に対する応答の絶対値|ｚ|のｔ＞１における最大値をｚrefmaxとする。また、数９９に対する応答の絶対値|ｚ|のｔ＞η(１)における最大値をｚmaxとする。すると、ｚmax／ｚrefmaxが入力値整形によりどれだけ残留振動が低減されたかを表す指標になる。そこで、ａをω0／１０から１０ω0の間で変えながら運動方程式をDormand-Prince法により解いた。どのようにｚmax／ｚrefmaxが変化するのかを、Ｃｈｏの方法による結果とともに図１４に示す。
【数９８】
【数９９】
【００６１】
図１４に示すように、本発明の手法では、固有振動数が初期固有振動数ω0に対して単位時間あたりで１０倍も変化するような場合でも、残留振動が３％以下にまで低減されていることが分かる。また、Ｃｈｏの方法と比べると、固有振動数が高速に変化する場合でも効果的に残留振動が抑制されていることが分かる。
【００６２】
（効果）
本発明の手法では、多自由度の複雑な系であっても、入力値整形に必要な関数を補間などの方法により求めることで計算時間の短縮を狙うことができる。α(t)は減衰などによるエネルギ消散量に関連しており、インパルス応答が分かれば容易にα(t)の関数形状を想像することができる。例えば、図７より、振幅が時間とともに増大していく場合には、対応するα(t)は常に１より大きくなり、振幅の変化が小さくなるとα(t)は１に近づいていく。また、η−１(t)についても瞬間的な固有周期の時間変化率に関連している。図７に示したように固有周期が短くなる場合には、２つ目のインパルスの値は大きくなるようにせねばならず、η−１(t)は常に１以上となる。また、固有周期をＴ(t)（数１００）とすると、その変化率は数１０１のように表され、時間の経過とともに固有周期の変化が小さくなることが分かる。
【数１００】
【数１０１】
このことから、時間の経過ともにη−１(t)が１に収束していくことが予想され、実際、図８ではそのようになっている。このように、固有周期の変化をあらかじめ知ることができればη−１(t)の形状を予測できる。すると、これらの関数は補間などの手法により得ることができ、設計者が補間に必要なだけの点数を選ぶことが可能になる。関数の形状さえ再現できるのであれば、不必要に時間間隔を細かく区切る必要が無くなる。このことから、本発明の手法では入力値を整形するのに必要な演算量（作業量）を低減することが可能であると言える。
【００６３】
（実施例２：水平２関節型ロボットアーム）
次に、実施例２として、水平２関節型ロボットアームへの適用例について説明する。
【００６４】
図１５に示す水平面内で動作する２関節ロボットアーム２０について、残留振動を抑制するための加減速パターンを生成し、数値計算と実験により上述にて説明した入力値整形の有効性を検証した。
【００６５】
この検証にて用いた実験装置（実験モデル）を図１６に示す。この実験装置は、第１リンク１１を水平面内にて回動させる第１関節２１(Joint 1)にＡＣサーボモータRSF-14A-100(ハーモニック・ドライブ・システムズ社)を用い、第２リンク１２を水平面内にて回動させる第２関節２２(Joint 2)にＡＣサーボモータRSF-14A-50(同社)を用いた水平面内で動作する２関節ロボットアーム２０である。これらのＡＣサーボアクチュエータには波動歯車装置が用いられている。各関節のモータＭ１、Ｍ２は、それぞれＡＣサーボドライバHA-655(ハーモニック・ドライブ・システムズ社)により図１７の表に示す設定で位置制御モードにより制御されており、外部指令装置からパルスを送ることで駆動する。第１関節２１は１パルスあたり4.5×10−3度、第２関節は9.0×10−3度回転する。外部指令装置にはDS1104 R&D コントローラボード(dSPACE 社)のDAC出力を用い、１秒あたり最大で2.5×104パルスを同時に２つのＡＣサーボドライバに送ることができる。また、動作終了後のアーム先端の残留振動の大きさの測定には、レーザー変位計HLC105B-HK(SUNX 社)を用いた。なお、この測定値はフィードバックすることなく、測定のためだけに用いた。
【００６６】
（運動方程式）
関節ｊの回転角度をθj，その指令値をθ0j，リンクｌの質量をｍｌ，リンクｌの関節ｊ周りの慣性モーメントをＪj，リンクｌの長さをｂｌ，関節ｊからリンクｌの質量中心までの距離をｌj，関節ｊのねじり剛性をｋjとする。運動方程式は、数１０３〜数１１１の条件に基づいて数１０２のように表される。
【数１０２】
【数１０３】
【数１０４】
【数１０５】
【数１０６】
【数１０７】
【数１０８】
【数１０９】
【数１１０】
【数１１１】
【００６７】
数５〜数１０によるテイラー展開により、参照指令値をθref＝ [θref1 θref2]Tとすると、水平２関節型ロボットアームの運動方程式は数２１で表される。ただし、各パラメータは数１２〜数１５より、数１１２〜数１１５のようになる。ただし、数１１６〜数１２２に示す関係にある。
【数１１２】
【数１１３】
【数１１４】
【数１１５】
【数１１６】
【数１１７】
【数１１８】
【数１１９】
【数１２０】
【数１２１】
【数１２２】
なお、水平多関節型ロボットアームの場合には重力項が無いため、数１８よりψ＝φである。
【００６８】
（パラメータ同定）
本発明の手法により加減速パターンを生成するために必要なパラメータの同定方法について述べる。まず、電子はかりによる測定と幾何形状から求まるパラメータを図１８の表に示す。ここで、未知のパラメータはＪ１、ｋ１、ｋ２、Ｃvである。これらは以下に説明するようにして同定した。
【００６９】
（モーダルパラメータの同定）
まず、全てのモータをサーボオンにした状態で、第１関節２１に対する指令角をθ01＝０，θ02＝θr2として固定したときの自由応答を測定した。ここで、θr2は、数１２３のように表される。
【数１２３】
自由応答は、図１９のように、加速度ピックアップをアーム先端から9mmのところに貼り付け、その裏側をインパクトハンマにより加振し、アーム先端の加速度を測定した。θ02＝θr2のときにＦＦＴアナライザにより得られたアクセレランスをＧr(ωk)とする。なお、サンプリング周波数を256Hz，計測時間を16秒，ωk＝2πｆk，ｆkを0，0.0625，0.125，...，128Hzとした。ここで、一般粘性減衰をもつ２自由度系のアクセレランスＬr(ω)（数１２４）に対し、Ｇr(ωk)を1〜100Hzの領域で曲線適合させるため、数１２５にて表される値が最小となるようパラメータＵri，Ｖri，ωrni，ζriを信頼領域Reflective法により求めた。
【数１２４】
【数１２５】
【００７０】
一例として、θ02＝θ42＝π/4radのときに計測した伝達関数Ｇ4(ωk)と、求まったパラメータをもとに求めた伝達関数Ｌ4(ω)を図２０に示す。
なお、θ02＝π/2，7π/12radのときは、鉛直方向の１次固有振動数と水平方向の２次固有振動数が近く、インパクトハンマによる打撃によって鉛直方向の振動が少なからず励起されるとともに、加速度ピックアップが鉛直方向の加速度をも拾ってしまうため、曲線適合は一般粘性減衰をもつ３自由度系のアクセレランスＬr(ω)（数１２６）を用いて行った。
【数１２６】
【００７１】
（慣性モーメントとねじり剛性の同定）
次に、ωrniをｉ次モードの固有振動数とみなし、このパラメータをもとにＪ1，ｋ1，ｋ2を以下のように同定した。数５において数１２７に示すようにしたときの偏差角φに関する運動方程式（数１２８）より実固有値解析によって求まる固有振動数をω'rniとする。
【数１２７】
【数１２８】
そして、数１２９に示す値が最小となるよう信頼領域Reflective法によりＪ1，ｋ1，ｋ2を図２１に示す表のように同定した。
【数１２９】
アクセレランスの曲線適合により求めた固有振動数ωrniと、同定したＪ1，ｋ1，ｋ2を運動方程式（数１３０）に代入し、実固有値解析により求まる固有振動数ω'ni(θ2)を図２２に示す。
【数１３０】
なお、第１関節２１に用いられているモータＭ１のねじり剛性は、ねじり角が小さい場合には4.7×103N・m/radである。また、第２関節２２では3.4×103N・m/radである。同定したねじり剛性ｋ1，ｋ2がこれらの値よりも小さいのは、関節に用いられている軸の軸径が小さいことにより機械的剛性が低くなっている部分があることと、サーボ機構により系全体の剛性が下がっていることによる。
【００７２】
（粘性減衰行列の同定）
次に、粘性減衰行列Ｃvを求める。ζriをｉ次モードのモード減衰比とみなし、θ02＝θr2のときの粘性減衰行列が、定数αr，βrを用いて数１３１というRayleigh減衰の形で与えられるとすると、定数αr，βrは、ｉ次モードに関して数１３２という関係を満たす。これより、１次のモーダルパラメータωrn1，ζr1と、２次のモーダルパラメータωrn2，ζr2を用いて、定数αr，βrがそれぞれ数１３３、数１３４と求まる。
【数１３１】
【数１３２】
【数１３３】
【数１３４】
数１３１、数１３３、数１３４よりＣvrが求まるので、その平均をとることで数１３５のようにＣvを決めた。
【数１３５】
【００７３】
（座標系の定義）
アーム先端の面内振動を評価するため、図２３のようにベースから見た座標系ΣB，アーム先端の指令位置から見た座標系ΣH，動作終了後のアーム先端の位置から見た座標系ΣEを定める。なお、θE＝[θ1E θ2E]Tは動作終了時における関節の角度である。
【００７４】
図２３より、座標系ΣBにおけるアーム先端の位置は、数１３６、数１３７のようになる。
【数１３６】
【数１３７】
座標系ΣHにおけるアーム先端の位置は、同次変換（数１３８）により、数１３９、数１４０となる。
【数１３８】
【数１３９】
【数１４０】
この座標系ΣHは動作中のアーム先端の振動を評価するために用いる。同様にして，座標系ΣEにおけるアーム先端の位置は、数１４１、数１４２となる。
【数１４１】
【数１４２】
座標系ΣEにおけるアーム先端の位置は、レーザー変位計により直接測定できるもので、動作終了後のアーム先端の残留振動を評価するために用いる。
【００７５】
（突き出し動作）
次に、図２４に示す突き出し動作について考える。動作開始時の各関節の角度θSは数１４３にて表され、動作終了時の関節の角度θEは数１４４にて表される。
【数１４３】
【数１４４】
【００７６】
最大角加速度を4πrad/s2，最大角速度をπrad/sとし、２つの関節が同時に動作終了するようにすると、最短時間制御は図２５に示すような指令値となる。これを参照指令値θrefとし、対応する加減速パターン（数１４５），速度パターン（数１４６）についても図２５に示す。
【数１４５】
【数１４６】
【００７７】
ここで、ｉ次モードの疑似固有振動数と疑似減衰比をそれぞれωi(t)＝|di|，ζi(t)＝−real[di]/ωiとすると、動作中の疑似固有振動数と疑似減衰比は図２６のように変化する。１次固有振動数と２次固有振動数は、動作開始時にはそれぞれ7.9 Hz，18.3 Hzであるが、動作終了時にはそれぞれ6.2 Hz，28.0 Hz となる。
【００７８】
（近似の妥当性）
この突き出し動作において、上述にて説明した加減速パターンを導出する際に行った近似の妥当性を検証する。元々の非線形な運動方程式（数４）、剰余項を無視して線形化した運動方程式（数２１）、非対角項を無視することによって導いたモードごとの非連成な運動方程式（数３４）をそれぞれ数値計算により解いた。その結果得られる偏差角をそれぞれψ＝[ψ1 ψ2]T，ψL＝[ψL1 ψL2]T，ψM＝[ψM1 ψM2]Tとし、図２７に示す。また、非線形な運動方程式（数３４）の解ψからの誤差を数１４７、数１４８のように定義し、図２８に示す。
【数１４７】
【数１４８】
なお、数値計算にはDormand-Prince法を用いた。図２８より、線形化したことによる誤差が５％以下であり、モード分解をしても誤差が５．５％以下であることから、この突き出し動作における数３４の近似は妥当であると言える。
【００７９】
（加減速パターンの生成）
加減速パターンを生成するにあたって、数１４９を解析的に求めることは困難であるため、差分を用いて数１４９を計算する。しかしながら、図２５に示す加減速パターンは時刻ｔに関して不連続であるため、微小時間ΔtPを用いて数１５０とすると、Ｐ(t)が不連続になる時刻ｔにおいて、数１５１の各要素が他の時刻のときと比べて非常に大きな値をとり、生成される加減速パターンは残留振動を抑制しなくなる。
【数１４９】
【数１５０】
【数１５１】
そこで、数１５１を数１５２とする。
【数１５２】
ただし、med関数は引数として与えられた行列のうち、フロベニウスノルムが中央値をとる行列を返す関数である。このように数１４９を定義しなおすと、ΔtPを小さくしても、Ｐが不連続な点において数１４９の各成分が異常に大きな値をとることがなくなる。
【００８０】
この数１４９を用いて、数６４により得られる加速度パターン（数５９）と対応する（数６０）（数６１）を図２９に示す。ただし，ΔtP＝0.001sとした。整形前の図２５と整形後の図２９とを比べることにより、両者の速度パターンと指令値に大きな差異がないことが分かる。よって、この場合にはモード形状の時間推移は整形後も変わらないものとみなすことが妥当であると言える。
【００８１】
比較のため、Input Shaperの中で最も基本的なZV Shaperにより数１５３を整形した加
減速パターン（数１５４）を図３０に示す。なお，ZV Shaperのパラメータとして、同定した１次モードのモーダルパラメータの平均値を数１５５、数１５６のように用いた。
【数１５３】
【数１５４】
【数１５５】
【数１５６】
つまり、この加減速パターンは１次モードの固有振動数が6.9 Hz，減衰比が0.037である線形時不変系の残留振動を抑制する入力に相当する。時変系である２関節型ロボットアームに適用しても、ある程度残留振動が抑制されることが期待されることから、本発明の手法の比較対象として以下のように取り上げる。
【００８２】
ここで、動作を開始してから最後に指令値を送るまでの時間を動作時間とし、各加減速パターンにより生成される指令値の動作時間を比較する。参照指令値ではｔ＝0.834sで動作終了するのに対し、整形後の指令値ではｔ＝0.915sで動作終了する。この参照指令値に対する動作終了時刻の遅れは、動作終了時の１次モードの半周期π/ω1E＝0.081sに相当する。ここで、ωiEは動作終了時のｉ次モードの固有振動数である。本発明の手法では、一般に参照指令値の動作時間に対して１次固有周期の半分π/ω1Eだけ動作が遅れる。これは、モードごとに２パルスで入力値整形を行うため、最も固有周期の長い１次モードにおいて入力が終わるまでにかかる時間が動作時間となるからである。そのため、高次のモードを扱う場合でも１次固有周期の半分だけ動作が遅れることになる。これに対し、Ｃｈｏの方法などの２パルスのConvolved型のInput Shaperでは、一般に、残留振動を抑制したいモードの固有周期の合計の半分（数１５７）だけ動作終了時刻が遅れる。
【数１５７】
そのため、本発明の手法の方が動作終了時刻を短縮できることが分かる。なお、ZV Shaperでは平均的な固有振動数を用いて加減速パターンを整形するため、動作終了時刻はπ/ωZVだけ遅れる。すなわち、この突き出し動作の場合はｔ＝0.907sで動作が終了する。
【００８３】
（結果と考察）
生成した指令値θshapedを与えた場合に、参照指令値θrefを与えた場合やZV Shaperにより生成した指令値θZVを与えた場合と比べて残留振動がより抑制されることを数値計算と実験により検証した。なお、以下に示す数値計算による結果は、数４をDormand-Prince法により解いて得られるθから計算したものである。
【００８４】
まず、動作終了後の残留振動を評価するため、数値計算により求めた動作終了後のyEを図３１に示し、実験においてレーザー変位計により測定したyEを図３２に示す。図３１、図３２より、実験結果が数値計算の結果をよく再現していることが分かるとともに、本発明の手法がZV Shaperと比べてより効果的に残留振動を抑制していることが分かる。ここで、動作開始から振動振幅が0.25mm以下に収まるまでの時間を位置決め時間とすると、実験結果より、θrefを与えた場合には位置決め時間が2.5秒以上、θZVを与えた場合には1.61秒となった。それらに対し、θshapedを与えた場合には、動作終了時には振動振幅が0.25mm以下となっており、位置決め時間は0.92秒となった。これより、本発明の手法を用いると位置決め時間が短縮され、作業効率を向上させられることが分かる。
【００８５】
次に、動作中の振動を評価するため、数値計算により求めた動作中と動作終了後のxHとyHを図３３に示す。図３３より、θshapedを与えると動作中の振動も抑制されることが分かる。これは、時変性を考慮して入力値整形をしているためである。それに対し、θZVを与えた場合には、動作中である0.3〜0.58秒付近において偶然にも振動が収まっているが、時変性を考慮せずに入力値整形をしているため、動作中のその他の期間と動作終了後において振動が生じる結果となっている。
【００８６】
最後に、数値計算の結果より、座標系ΣBにおいてアーム先端が描く軌跡を図３４に示す。θshapedを与えたときのアーム先端の軌道は、必ずしも参照指令値θrefを与えたときと同じ軌道を描くとは限らないことが図３４より分かる。そのため、アーム先端の軌道に制約がある場合などには、参照指令値θrefを変えて加減速パターンを生成しなおす必要がある。この突き出し動作の例の場合には、アーム先端がyB＞０の領域に大きく侵入していることが分かる。これは、図２５、図２９に示すように、第２関節の参照加速度（数１５８）に比べて、生成した第２関節の加速度パターン（数１５９）では動作初期において大きく加速し、第１関節よりも速く動作するためである。このため、例えばyH＝0.01mの位置に障害物があるような場合には、参照指令値の第２関節の角速度を落とすなどしてから加減速パターンを生成しなおす必要がある。
【数１５８】
【数１５９】
【００８７】
（動作時間と残留振動）
非減衰１自由度線形時不変系において、固有周期の整数倍の時間だけ一定外力を与えると残留振動が生じないということから、図２５のような一定加速度により加減速する加減速パターンを用いても、加速時間と減速時間を調整することにより残留振動をある程度まで抑えられることが予想される。同様に、時変性を考慮しないZV Shaperを用いても加減速時間と系の固有周期の関係により残留振動が効果的に抑えられる場合があると考えられる。しかし、これらの方法では系の固有周期により動作時間を決めなければならないため、必ずしも位置決め時間を短縮することができるとは限らない。それに対し、本発明の手法により生成する加減速パターンは、時変性を考慮しているため、系の固有周期と加減速時間の関係によらず残留振動を抑制でき、位置決め時間を短縮できると考えられる。このことを示すため、動作時間と発生する残留振動の大きさの関係を数値計算と実験により調べた。
【００８８】
まず、一定加速度により加減速する動作について考える。θdを数１６０とする。
【数１６０】
参照指令値として最大角速度をvmax，最大角加速度をＡmaxとし、２つの関節の動作が同時に終了する最短時間制御を行う。θd＜v2max/Ａmaxの場合には図３５のような三角形速度パターンとなり、θd≧v2max/Ａmaxの場合には図３６のような台形速度パターンとなる。このとき加速時間(または減速時間)ｔa、動作時間ｔmは数１６１、数１６２のようになる。
【数１６１】
【数１６２】
【００８９】
ここで、vmax＝πrad/sとし、最大角加速度Ａmaxを与えると、ｔa，ｔm，および数１６３が決まる。この加減速パターンに対応する参照指令値θrefを与えたとき、数値計算により得られる動作終了後(ｔ>ｔm)のアーム先端の最大変位max[|yE|]を図３７に示す。ここで、横軸は動作終了時の１次固有周期に対する加速時間の比（数１６４）である。
【数１６３】
【数１６４】
図３７より、加速時間が１次固有周期の整数倍に近いと発生する残留振動が小さくなることが分かる。しかし、２関節ロボットアームでは非線形性や時変性により、加速時間が１次固有周期の整数倍であっても少なからず残留振動が発生することが分かる。
【００９０】
次に、数値計算により得られる動作終了後のアーム先端の最大変位max[|yE|]を、横軸を動作時間ｔmとして図３８に示す。また、ZV Shaperを適用した場合の最大変位と、本発明の手法を適用した場合の最大変位も示す。なお、最高角速度vmaxをπrad/sとしているので、参照指令値の最高角加速度を大きくして最も制御時間が短くなるようにしても、一定加速度の加減速パターンでは、数１６５だけ動作に時間がかかる。同様にして、ZV Shaperにより整形した加減速パターンでは、数１６６だけ動作に時間がかかり、本発明の手法により生成された加減速パターンでは数１６７だけ動作に時間がかかる。
【数１６５】
【数１６６】
【数１６７】
ここで、実験においてレーザー変位計により測定した最大変位を図３９に示す。図３８、図３９より、動作時間が短い場合には本発明の手法がZV Shaperと比較して動作時間によらず効果的に残留振動を抑制できることが分かる。図３９より、動作時間によっては本発明の手法よりも発生する残留振動の大きさがZV Shaperを用いた方が小さくなる場合があることが分かる。しかし、アーム先端の負荷が変わるなどして固有周期が変化すれば、残留振動が小さくなる動作時間は変化する。そのため、残留振動が生じない動作時間を見つけるのは困難である上、動作時間が限られてしまうため、実際にZV Shaperを多関節型ロボットアームに適用して任意の動作の残留振動を抑制するのは難しい。これに対し、本発明の手法で加減速パターンを生成すると、残留振動を抑制しながら動作時間を短縮できることが分かる。
【００９１】
なお、本発明の手法において、動作時間が短い場合、すなわち、角加速度が大きい場合には、角加速度が小さい場合と比べて残留振動の抑制効果が小さいことが図３８より分かる。本発明の手法も含む各種Input Shaping法では、完全に打ち消し合わせられない振動的な応答が残る場合に、加振力に相当する角加速度が大きいことにより残留振動が大きくなる。時変性を考慮したにも関わらず本発明の手法により振動的な応答が残るのは、加減速パターンの整形前後においてモード形状の時間推移が比較的大きく変わったためだと考えられる。Ａmax＝61.8 rad/s2のときの参照指令値の加減速パターンとそれに対応する速度パターン、指令値を図４０に示す。また、この参照指令値をもとに本発明の手法で生成した加減速パターンとそれに対応する速度パターン、指令値を図４１に示す。両者の速度パターンを比較すると、特に減速期間において速度パターンが異なることが分かる。しかし、加減速パターンを生成する際には加減速パターンの整形前後においてモード形状の時間推移が変わらないことを前提としているため、この前提をもとに生成した加減速パターンでは残留振動が完全に抑制できないということになる。
【００９２】
ここで、図３８と図３９を比較することにより、動作時間が長い場合、すなわち、角加速度が小さい場合には、数値計算による結果と比べて実験ではあまり残留振動が抑制されていないことが分かる。これは、指令角の量子化の精度が粗いためである。動作終了前後における関節１に対する理想的な指令角θshaped1と、実際にサーボドライバに送信されるデジタル化された指令角を図４２に示す。図４２（ａ）には角速度が大きい場合の指令角を示し、図４２（ｂ）には角速度が小さい場合の指令角を示している。図４２より、角加速度が小さい場合には、動作終了直前に非常にゆっくりと回転するように指令をするが、量子化の精度が粗いために、断続的に角度を指令することになる。このため、動作時間が長い場合に残留振動が発生してしまっていると考えられる。このことから、本発明の手法を実際にロボットアームに適用する場合には量子化の精度に注意することが望ましい。
【００９３】
（振り回し動作）
図４３に示すアームを振り回す動作により、ZV Shaperとの違いを明確にする。この動作は、図２４に示す突き出し動作において第２関節２２の回転方向を逆向きにしたものである。つまり、第２関節２２への指令角θ02の符号を反転させただけであり、固有振動数の時間推移は等しくなる。しかし、１次モードに寄与するトルクが大きくなるため、その分だけ残留振動が大きくなり、時変性を考慮しない従来法との違いが顕著になる。
【００９４】
ここで、最大角加速度を4πrad/s2，最大角速度をπrad/sとし，２つの関節が同時に動作終了するような最短時間制御を参照指令値θrefとする。これをもとに、ZV ShaperでθZVを、本発明の手法でθshapedを生成し、数値計算と実験により残留振動の大きさを調べた。yEについて、数値計算の結果を図４４に、実験結果を図４５に示す。これより、時変性を考慮しないZV Shaperと比べて、本発明の手法の方がより効果的に残留振動を抑制できることが分かる。
【００９５】
最大角速度をπrad/s，最大角加速度をＡmaxとし、２つの関節が同時に動作終了する最短時間制御を参照指令値θrefとする。Ａmaxを変化させて、この参照指令値をもとにZV Shaperで生成した指令値θZVと、本発明の手法で生成した指令値θshapedを与えたとき、残留振動のアーム先端の最大変位を数値計算により調べた結果を図４６に示し、実験により調べた結果を図４７に示す。突き出し動作と比べると、１次モードに寄与するトルクが大きいために、実験では残留振動の振幅が全体的に大きくなってしまっている。しかし、時変性を考慮しないZV Shaperでは動作時間により残留振動が抑制される場合とそうでない場合があるのに対し、本発明の手法では動作時間によらず残留振動が抑制されている。このことから、本発明の手法が多関節型ロボットアームに有効であると言える。
【００９６】
（折りたたみ動作）
上述の説明にて述べたとおり、本発明の手法では全てのモードの残留振動を抑制する。しかしながら、これまでに見てきた動作例では２次の振動モードがほとんど励起されなかった。そこで、２次モードの振動も抑制されることを示すため、図４８に示すアームを折りたたむ動作により発生する残留振動を、実験によりレーザー変位計で測定した。測定した座標系ΣEにおけるアーム先端の位置を図４９に示す。図４９よりyE方向の変位には２次モードの成分が多く含まれていると考えられる。そこで、yEのパワースペクトルを図５０に示す。図５０より、２次モードについてもピークレベルが下がっており、本発明の手法により複数の振動モードが抑制されることが分かる。
【００９７】
なお、図５０より、残留振動を抑制し、偏差角が小さくなると、固有振動数が変化することが分かる。これは、波動歯車装置の非線形剛性や、サーボ機構によるものだと考えられる。本発明の手法では、このような剛性の非線形性を厳密に考慮することはできない。しかしながら、上述にて説明した３つの動作例による実験結果から、剛性が線形であるとしてアクセレランスなどをもとにパラメータ同定をすることである程度まで振動が抑えることができることが分かる。
【００９８】
（実施例３：垂直２関節型ロボットアーム）
次に、実施例３として、垂直２関節型ロボットアームへの適用した例について説明する。具体的には、図５１に示す鉛直面内で動作する２関節ロボットアーム２０について、残留振動を抑制するための加減速パターンを生成し、数値計算により重力がある場合でも本発明の手法が有効であることを検証した。
【００９９】
（運動方程式）
関節ｊの回転角度をθj，その指令値をθ0j，リンクｌの質量をｍｌ，リンクｌの質量中心まわりの慣性モーメントをＪGｌ，リンクｌの長さをｂｌ，関節ｊからリンクｌの質量中心までの距離をｌj，関節ｊのねじり剛性をｋjとする。運動方程式は数１６８のようになる。ただし、数１６９〜数１７９の関係にある。
【数１６８】
【数１６９】
【数１７０】
【数１７１】
【数１７２】
【数１７３】
【数１７４】
【数１７５】
【数１７６】
【数１７７】
【数１７８】
【数１７９】
また、ｇ＝9.8m/s2は重力加速度であり、図５１に示す方向に重力が働いているものとする。数５〜数１０によるテイラー展開により、参照指令値をθref＝[θref1 θref2]Tとすると、垂直２関節型ロボットアームの運動方程式は数２１で表される。ただし、各パラメータは数１１〜数１５より、数１８０〜数１８３のようになる。
【数１８０】
【数１８１】
【数１８２】
【数１８３】
なお、数１８４〜数１９０の関係にある。
【数１８４】
【数１８５】
【数１８６】
【数１８７】
【数１８８】
【数１８９】
【数１９０】
【数１９１】
【数１９２】
ここで、減衰力は各関節の偏差角の変化率に比例して働くものとすると、粘性減衰行列Ｃvは、数１９３のように表される。なお、本実施例３では、図５２の表に示す諸元を用いて以下の数値計算を行った。
【数１９３】
【０１００】
（突き出し動作での加減速パターンの生成）
図２４に示す突き出し動作について考える。重力による釣り合いの位置を考慮すれば動作開始時の指令値θ0S＝ [θ01S θ02S]Tは、数１９４を満たすθ0Sとなる。ここで数１９５である。
【数１９４】
【数１９５】
同様にして、動作終了時の指令角θ0E＝[θ01E θ02E]Tは、数１９６を満たすθ0Eとなる。
【数１９６】
図２４のように、θ1S＝7π/24＝0.916rad，θ2S＝−7π/12＝−1.83rad，θ1E＝θ2E＝0radである場合には。θ01S＝0.926rad，θ02S＝−1.83rad，θ01E＝0.016rad，θ02E＝0.004radとなる。このことを考慮し、最大角加速度を4πrad/s2，最大角速度をπrad/sとすると、最短時間制御は図５３に示すような指令値となる。これを参照指令値θrefとし、対応する加減速パターン（数１９７），速度パターン（数１９８）についても図５３に示す。
【数１９７】
【数１９８】
【０１０１】
Ｋ-1(t)ｇref(t)の時間微分を解析的に求めるのは困難であるので、数１５２と同様に数１９９、数２００と定義する。
【数１９９】
【数２００】
数１９９、数２００、および数１５２を用いて、数６４により得られる加速度パターン（数５９）と対応する（数６０）、（数６１）を図５４に示す。ただし、ΔtP＝0.001s，ΔtG＝10-5sとした。整形前の図５３と整形後の図５４とを比べることにより、両者の速度パターンと角度指令値は大きく違わないことが分かる。これにより、モード形状の時間推移は整形後も変わらないものとみなすことが妥当であると言える。
【０１０２】
（結果と考察）
参照指令値θrefを与えた場合と比べて、整形した加減速パターン（数５９）に対応する角度指令値θshapedを与えたときの方が、残留振動が抑制されることを数値計算により示した。θrefを与えた場合とθshapedを与えた場合について、それぞれDormand-Prince法により数４を数値的に解いた。解いた結果得られるθから計算した、動作終了後のアーム先端の座標系ΣBにおけるy方向の変位を図５５に示す。これより、本発明の手法が、重力を考慮しなければならない垂直２関節ロボットアームの残留振動抑制にも有効であることが分かる。
【０１０３】
また、数値計算の結果より、動作中の各関節の角速度θを図５６に示し、釣り合いの位置からの偏差角ψ＝[ψ1 ψ2]Tを図５７に示す。これらより、水平２関節型ロボットアームの場合と同様に、動作中の振動が抑制されているのが分かる。
【０１０４】
これらの数値計算の結果より、釣り合いの位置からの偏差角ψを数１８と定義し、ψに関する運動方程式を数２１のように書くことで、水平２関節型ロボットアームと同じように残留振動を抑制する加減速パターンを生成できることが分かる。
【０１０５】
本発明によれば、加速または減速運動に伴って運動体に生じるｎ自由度かつ時変の振動を、インプットシェイピング法を用いて抑制する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第２の入力を決定するという手法を採用している。
【０１０６】
また、本発明では、ｎ自由度かつ時変の振動におけるモード間の相互作用によるエネルギの移り変わりをゼロと見なして、固有振動数および減衰比の時間変化によるモード間のエネルギの移り変わりを考慮して、第１の入力に対する第２の入力の大きさを決定するという手法を採用していると言うこともできる。
【０１０７】
また、別の観点からは、本発明では、ｎ自由度かつ時変の振動における複数のモード間の相互作用によるエネルギの移り変わりを無視し、加速または減速運動を行っている間に運動体自体の姿勢が変化することにより生じるコリオリがした仕事を考慮して、第１の入力に対する第２の入力の大きさを決定するという手法を採用していると言うこともできる。
【０１０８】
このような手法を採用していることにより、各振動モードにおいて、第１の入力に対応する入力を打ち消す第２の入力に対応する入力の大きさを個別に算出して、振動モードごとに算出された入力を合成することで第２の入力の大きさを決定することが可能となる。
【０１０９】
そのため、本発明によれば、第１の入力によって生じたｎ自由度かつ時変の振動（第１の応答）を抑制できる第２の入力を算出することが可能となる。
【０１１０】
よって、ロボットアーム、工作機械、およびクレーンなど、その動作中に姿勢変化を伴うような動作を行う運動体に生じる振動、すなわち、ｎ自由度かつ時変の振動を、インプットシェイピング法を適用して効果的に抑制する振動抑制方法を提供することができる。
【０１１１】
なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。
【符号の説明】
【０１１２】
１０ ロボットアーム
２０ ロボットアーム
Ｊｎ 関節
Ｌｎ リンク
Ｐ１ 第１のインパルス
Ｐ２ 第２のインパルス
Ｐ３ 第１のインパルス列
Ｐ４ 第２のインパルス列
【特許請求の範囲】
【請求項１】
加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、
運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法。
【請求項２】
加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、
運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間の相互作用によるエネルギの移り変わりを無視し、加速または減速運動を行っている間に運動体自体の姿勢が変化することにより生じるコリオリがした仕事を考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法。
【請求項３】
加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、
運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動におけるモード間の相互作用によるエネルギの移り変わりをゼロと見なして、固有振動数および減衰比の時間変化によるモード間のエネルギの移り変わりを考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法。
【請求項４】
各振動モードにおいて、第１の入力に対応する入力を打ち消す第２の入力に対応する入力の大きさを個別に算出して、振動モードごとに算出された入力を合成することで第２の入力の大きさを決定する、請求項１から３のいずれか１つに記載の振動抑制方法。
【請求項５】
運動体が多関節型ロボットアームであって、ロボットアームに生じるｎ自由度かつ時変の振動を抑制する、請求項１から４のいずれか１つに記載の振動抑制方法。
【請求項１】
加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、
運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間のエネルギの移り変わりのうち、モード間の相互作用によるものを無視し、固有振動数および減衰比の変化によるものを考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法。
【請求項２】
加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、
運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動における複数のモード間の相互作用によるエネルギの移り変わりを無視し、加速または減速運動を行っている間に運動体自体の姿勢が変化することにより生じるコリオリがした仕事を考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法。
【請求項３】
加速または減速運動が行われる運動体において、加速または減速運動に伴って運動体に生じるｎ自由度（ｎは自然数）かつ時変の振動を抑制する振動抑制方法において、
運動体に付加された第１の入力によって運動体に生じる第１の応答に、第１の応答の変位が０となる時刻において、第２の入力を運動体に付加して生じる第２の応答を重ね合わせて、互いに打ち消し合うインプットシェイピング法を適用する場合に、ｎ自由度かつ時変の振動におけるモード間の相互作用によるエネルギの移り変わりをゼロと見なして、固有振動数および減衰比の時間変化によるモード間のエネルギの移り変わりを考慮して、第１の入力に対する第２の入力の大きさを決定する、振動抑制方法。
【請求項４】
各振動モードにおいて、第１の入力に対応する入力を打ち消す第２の入力に対応する入力の大きさを個別に算出して、振動モードごとに算出された入力を合成することで第２の入力の大きさを決定する、請求項１から３のいずれか１つに記載の振動抑制方法。
【請求項５】
運動体が多関節型ロボットアームであって、ロボットアームに生じるｎ自由度かつ時変の振動を抑制する、請求項１から４のいずれか１つに記載の振動抑制方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【図２９】
【図３０】
【図３１】
【図３２】
【図３３】
【図３４】
【図３５】
【図３６】
【図３７】
【図３８】
【図３９】
【図４０】
【図４１】
【図４２】
【図４３】
【図４４】
【図４５】
【図４６】
【図４７】
【図４８】
【図４９】
【図５０】
【図５１】
【図５２】
【図５３】
【図５４】
【図５５】
【図５６】
【図５７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【図２９】
【図３０】
【図３１】
【図３２】
【図３３】
【図３４】
【図３５】
【図３６】
【図３７】
【図３８】
【図３９】
【図４０】
【図４１】
【図４２】
【図４３】
【図４４】
【図４５】
【図４６】
【図４７】
【図４８】
【図４９】
【図５０】
【図５１】
【図５２】
【図５３】
【図５４】
【図５５】
【図５６】
【図５７】
【公開番号】特開２０１３−４０３７（Ｐ２０１３−４０３７Ａ）
【公開日】平成２５年１月７日（２０１３．１．７）
【国際特許分類】
【出願番号】特願２０１１−１３７７７６（Ｐ２０１１−１３７７７６）
【出願日】平成２３年６月２１日（２０１１．６．２１）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り （１）公益社団法人計測自動制御学会発行の「第１１回 公益社団法人 計測自動制御学会 システムインテグレーション部門 講演会 ＳＩ２０１０講演概要集」第１５０頁、ならびに「第１１回 公益社団法人 計測自動制御学会 システムインテグレーション部門 講演会論文集（ＤＶＤ）」において平成２２年１２月２３日に発表を行った。 （２）一般社団法人日本機械学会発行の「関西支部 第８６期定時総会講演会 講演論文集 Ｎｏ．１１４−１」第１−１８頁において平成２３年３月１９日に発表を行った。
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２２年度、独立行政法人新エネルギー・産業技術総合開発機構「次世代ロボット知能化技術開発プロジェクト 作業知能（生産分野）の開発 作業知能（生産分野）の研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【出願人】（５０４１３２２７２）国立大学法人京都大学 (1,269)
【Ｆターム（参考）】
【公開日】平成２５年１月７日（２０１３．１．７）
【国際特許分類】
【出願日】平成２３年６月２１日（２０１１．６．２１）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り （１）公益社団法人計測自動制御学会発行の「第１１回 公益社団法人 計測自動制御学会 システムインテグレーション部門 講演会 ＳＩ２０１０講演概要集」第１５０頁、ならびに「第１１回 公益社団法人 計測自動制御学会 システムインテグレーション部門 講演会論文集（ＤＶＤ）」において平成２２年１２月２３日に発表を行った。 （２）一般社団法人日本機械学会発行の「関西支部 第８６期定時総会講演会 講演論文集 Ｎｏ．１１４−１」第１−１８頁において平成２３年３月１９日に発表を行った。
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成２２年度、独立行政法人新エネルギー・産業技術総合開発機構「次世代ロボット知能化技術開発プロジェクト 作業知能（生産分野）の開発 作業知能（生産分野）の研究開発」委託研究、産業技術力強化法第１９条の適用を受ける特許出願
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【出願人】（５０４１３２２７２）国立大学法人京都大学 (1,269)
【Ｆターム（参考）】
[ Back to top ]