(57)【特許請求の範囲】
【請求項１】
先端で作業を行うアームを伸縮方向に移動する第２軸と、前記第２軸を前後に移動する第１軸からなる機械の制御装置であって、モーション指令を解読し第１移動指令を演算する第１移動指令演算部と、第２移動指令を演算する第２移動指令演算部とを備えたコントローラと、前記第１軸を駆動する第1モータと、前記第１移動指令に基づき前記第1モータを駆動する第１サーボドライバと、前記第２軸を駆動する第２モータと、前記第２移動指令に基づき前記第２モータを駆動する第２サーボドライバと、前記第１サーボドライバあるいは前記コントローラ内に備えた振動抑制制御部と、前記コントローラ内に前記第２軸の位置に応じて前記アームの振動周波数を演算する振動周波数演算部と、を備えた制御装置において、
前記振動周波数演算部が、アーム先端の基本質量とアーム質量と初期状態のアームの基本長とを用いて計算した基本周波数に対して、アーム基本長とアーム長の変化分との比を用い演算した結果の平方根をとった補正係数を前記基本周波数に掛けて計算したアームの振動周波数を前記振動抑制制御部に振動周波数パラメータとして設定することを特徴とする制御装置。
【請求項２】
前記振動周波数演算部が、アーム先端の基本質量とアーム質量と初期状態のアームの基本長とを用いて計算した基本周波数に対して、アーム基本長とアーム長の変化分との比の１次式または２次式または３次式で演算した補正係数を前記基本周波数に掛けて計算したアームの振動周波数（ｆ）を前記振動抑制制御部に振動周波数パラメータとして設定することを特徴とする請求項１に記載の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は低剛性機械の振動を抑制する制御装置に関する。
【背景技術】
【０００２】
従来、低剛性機械の振動抑制には例えば特許文献１がある。
図５により従来技術を説明する。図5で示されるように、機械は第１サーボドライバと第１モータからなる第１軸と第２サーボドライバと第２モータからなる第２軸で構成される。この機械の動作は、第２軸によってアームが伸縮し、第１軸によってアームが前後する。アームの前後によってアームは加振され、アームの伸縮によってアーム先端の振動周波数が変化する。この振動は、アーム先端からモータ間の機構によって吸収されてしまうため、モータを介してフィードバック信号を得ているサーボドライバには、振動していることが検出できない。しかし、この機械は、アーム先端が仕事をするので、アーム先端の振動が問題になる。この場合、図4のように、第１軸を駆動する第１サーボドライバ内で、あるアームの状態における振動周波数に対して設定した振動抑制制御を使用する必要がある。
また、特許文献１の従来技術では指令器から払い出される指令を通常の台形指令や三角波指令ではなく、アームの振動が発生しづらい指令を払い出す方法が開示されている。図７において、上は速度指令信号であり、加速時間の７５％の時刻で加速度を最大とする。さらに減速時間の２５％の時刻で減速の加速度を最大とする。つまり、アームの加速開始すなわち起動から加速終了すなわち所定速度に達するまでの加速時間において、加速開始時刻から起算して、加速時間の７５％または約７５％の時間が経過した時刻に、加速度のピークが来るよう、アーム駆動部への加速指令を行う。また、アームの減速開始から減速終了すなわち停止までの減速時間においては、減速開始時刻から起算して、減速時間の２５％または約２５％の時間が経過した時刻に、加速度の負のピークが来るように、アーム駆動部への減速指令を行うものである。
図5のような機械では、例えば１０００ｍｍから１５００ｍｍへアームが伸びると、振動周波数は２０Ｈｚから１１Ｈｚへと2倍程度変化する。サーボドライバによる従来の振動抑制制御は、設定した特定の周波数に対して振動をダンピングするものである。許容している振動周波数のずれは１０％〜２０％程度でしかない。
【０００３】
【特許文献１】特開２０００−７９５８３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、従来の振動抑制制御はもっとも振幅が大きい状態に対してのみ振動抑制を考慮しているため、図５のように振動周波数が変化する機械に対しては、常には制振効果を得ることができない。
また特許文献１のように指令を変形して振動抑制効果を得る方法でも、図5のように振動周波数が変化する機械に対しては、常には制振効果を得ることができない。
本発明は、上記問題を解決するためになされたものであり、従来技術では、サーボドライバのみで実行していた振動抑制制御の設定を、コントローラと連携することにより、振動周波数が変化する機械に対しても常に制振効果を得る制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
請求項1記載の本発明は、先端で作業を行うアームを伸縮方向に移動する第２軸と、前記第２軸を前後に移動する第１軸からなる機械の制御装置であって、モーション指令を解読し第１移動指令を演算する第１移動指令演算部と、第２移動指令を演算する第２移動指令演算部とを備えたコントローラと、前記第１軸を駆動する第1モータと、前記第１移動指令に基づき前記第1モータを駆動する第１サーボドライバと、前記第２軸を駆動する第２モータと、前記第２移動指令に基づき前記第２モータを駆動する第２サーボドライバと、前記第１サーボドライバあるいは前記コントローラ内に備えた振動抑制制御部と、前記コントローラ内に前記第２軸の位置に応じて前記アームの振動周波数を演算する振動周波数演算部と、を備えた制御装置において、前記振動周波数演算部が、アーム先端の基本質量とアーム質量と初期状態のアームの基本長とを用いて計算した基本周波数に対して、アーム基本長とアーム長の変化分との比を用い演算した結果の平方根をとった補正係数を前記基本周波数に掛けて計算したアームの振動周波数を前記振動抑制制御部に振動周波数パラメータとして設定することを特徴とするものである。
請求項２記載の本発明は、請求項１に記載の制御装置において、前記振動周波数演算部が、アーム先端の基本質量とアーム質量と初期状態のアームの基本長とを用いて計算した基本周波数に対して、アーム基本長とアーム長の変化分との比の１次式または２次式または３次式で演算した補正係数を前記基本周波数に掛けて計算したアームの振動周波数を前記振動抑制制御部に振動周波数パラメータとして設定することを特徴とするものである。
【発明の効果】
【０００６】
本発明によると、振動抑制制御の設定を、コントローラとサーボドライバとを連携することにより、振動周波数が変化する機械に対しても常に制振効果を得られる制御装置を提供できる。
【発明を実施するための最良の形態】
【０００７】
以下、本発明の実施例を図を用いて説明する。
【実施例１】
【０００８】
図１は実施例１の構成図である。図1において、１は第１サーボドライバ、２は第２サーボドライバ、３はコントローラ、４は第1モータ、５は第２モータ、６は第１エンコータ、７は第２エンコーダである。また、３１はモーション指令解読演算部、３２は第１移動指令演算部、３３は第２移動指令演算部である。さらに、１１は振動抑制制御部、１２は第１位置制御部、２２は第２位置制御部である。次に動作について説明する。
コントローラの第1軸の第１移動指令演算部３２と第２軸の第２移動指令演算部３３は、モーション指令解読部３１で計算した第１軸と第２軸への移動量をもとに第１サーボドライバおよび第２サーボドライバへの移動指令を出力する。
ここで、図5に示すロボットアームでは、第１軸方向の振動の周波数はアーム長で変わることが分かっており、アーム長は第２軸の位置によって一意に決まるので、第２軸の位置に応じて、アームの振動周波数を出力する振動周波数演算部３４を新たにモーションコントローラ内に追加する。振動周波数演算部３４が出力する振動周波数パラメータ値を第１サーボドライバの振動抑制制御部１１に入力する。コントローラと第１サーボドライバ間をシリアル通信等のデータ伝送部で接続する場合は、シリアル通信での通常のパラメータ伝送方式にて、容易に振動周波数を振動抑制制御部１１に設定することができる。また、パルス列信号により移動距離を伝送する場合は、信号伝送路とは別にサーボドライバの外部接点信号をコントローラから駆動してサーボ内部の振動周波数パラメータを変更することにより実現できる。また、振動周波数演算部１１は、例えば、第２軸の移動量とアームの振動周波数との関係をあらかじめ計測してテーブルを作成し、入力した第２軸の移動量からメモリテーブルを引いて周波数を出力する方式で構成する。モーション指令解読部３１が移動指令演算部３３へ出力した第２軸の移動指令をもとに振動周波数演算部３４が第１軸方向の振動周波数を振動抑制制御部１１の振動周波数パラメータとして設定するので、アーム長が変化しても、振動抑制制御部１１が常に最適な動作をするため、常にアームの軸1方向への振動を抑制できる。
【実施例２】
【０００９】
次に本発明の実施例２について説明する。実施例２を図２で示す。図1との相違点は、図1では、第１サーボドライバのなかで構成した振動抑制制御１１をコントローラで実現している点である。コントローラのなかでモーション指令解読部３１が移動指令演算部３３へ出力した第２軸の移動指令をもとに振動周波数演算部３４が第１軸方向の振動周波数を振動抑制制御部１１の振動周波数パラメータとして設定するので、アーム長が変化しても、振動抑制制御部１１が常に最適動作ができることになるため、常にアームの第１軸方向の振動を抑制できる。また、振動抑制制御部１１をコントローラ内で構成しているので、振動周波数パラメータを第１軸へ伝送する必要がなく、第１サーボドライバは第２サーボドライバと同様な通常のサーボドライバでよいという効果がある。
【実施例３】
【００１０】
次に本発明の実施例３について説明する。実施例３を図3に示す。図1との相違点は、図1では、コントローラ内に構成した振動周波数演算部３４を第１サーボドライバ内で実現している点と第２サーボドライバの位置情報を第１サーボドライバへ通信部４１にて伝送している点である。第１サーボドライバ内では、前記通信部４１にて伝送された第２軸の位置情報を振動周波数演算部３４に入力して、第１軸の振動周波数を演算し、前記振動周波数演算部１１の振動周波数パラメータとして逐次設定する。なお、通信部４１はシリアル通信で実現できるし、パラレル信号で伝送することもできる。また、第２サーボドライバの出力接点と第１サーボドライバの入力接点とを接続し、第２サーボドライバ内の第２軸の位置情報に従った接点のON,OFF情報を出力する。第１サーボドライバは、ON,OFF情報から、第２サーボドライバ内の第２軸の位置情報を復元することができる。復元した位置情報に従って、第１軸の振動周波数を演算し、振動周波数演算部１１の振動周波数パラメータとして逐次設定することもできる。
【実施例４】
【００１１】
次に、本発明の実施例４を説明する。本発明の第１〜３の実施例では、振動周波数演算部１１のテーブルを作成するために実際の機械で取得すべきデータ点数が多く、作業量が増えてしまう。この作業量を軽減するため、第４の実施例では関数演算にて振動周波数演算部１１を構成する。なお、前記第１〜３の実施例ではすべて、振動周波数演算部３４を用いるので、下記の他の周波数演算方法はすべて前記第１〜３の実施例の制御系の構成にあてはまるので、以下第1の実施例の構成で代表して説明する。
下記のように、第1軸および第2軸の機械モデルを作成する。
図45のような機械系を考える。このとき、第2軸の動作によってアーム長が変化し、第1軸の動作によってアーム先端が振動する。このとき、アーム先端の振動をモデル化した機械モデルは、図6のようになる。この機械モデルによって、第2軸に対する移動指令から、第1軸の振動周波数を計算する関数式を作成する。このとき、アームの振動周波数は式（１）から計算することができる。
【００１２】
ｆ＝（１／２π）√（３ＥＩ／Ｍｌ３）                              ・・・（１）
ここで、
ｆ  アームの振動周波数
Ｅ  アームのヤング率（材質から決定する）
Ｉ  断面2次モーメント（アームの断面形状から決定する）
Ｍ  アーム先端に換算した総質量
ｌ  アーム長
Ｍは、レーリー法より式（２）から求めることができる。
Ｍ＝ｍ＋３３ｍｂ／１４０                                          ・・・（２）
式（１）、式（２）より、アームの振動周波数は式（３）で求めることができる。
ｆ＝（１／２π）√（３ＥＩ／（ｍ＋３３ｍｂ／１４０）ｌ３）        ・・・（３）
【００１３】
図5の機械系では、振動周波数の変化の原因としてアーム長ｌの変化が考えられる。初期状態のアーム長ｌ０からΔｌだけアーム長が変化すると考えると、アーム長の振動周波数は式（４）または式（５）で求めることができる。
ｆ＝（１／２π）√（３ＥＩ／（ｍ＋３３ｍｂ／１４０）（ｌ０＋Δｌ）３））・・（４）
ｆ＝（１／２π）√（３ＥＩ／（ｍ＋３３ｍｂ／１４０）ｌ３）
×√（ｌ３／（ｌ＋Δｌ）３）
＝ｆ０√（１／（１＋（Δｌ／ｌ））３）                            ・・・（５）  
ここで、以下のような系を仮定する。
基本周波数  
ｆ０＝（１／２π）√（３ＥＩ／（ｍ＋３３ｍｂ／１４０）ｌ３）        ・・・（６）  
アーム基本長                ｌ
ヤング率                    Ｅ＝２０６×１０９  （Ｐａ）
アームの断面2次モーメント  Ｉ＝３．５０×１０７  （ｍ４）
アーム先端の基本質量        ｍ＝５  （ｋｇ）
アームの質量                ｍｂ＝３０  （ｋｇ）
【００１４】
第１軸に対しては、通常の移動指令の他に作成した関数式によって計算した第１軸の振動周波数から振動抑制制御に必要なパラメータを出力する。すなわち、第１〜３の実施例と同様にモーション指令解読部３１が移動指令演算部３３へ出力した軸２の移動指令をもとに振動周波数演算部３４が第１軸方向の振動周波数を振動抑制制御部１１の振動周波数パラメータとして設定する。
振動周波数演算部１１を関数式（３）を用いて構成する本発明の実施例２によれば、数点の実際の機械でのデータのみで関数式のパラメータが決まり振動周波数を計算できるため、実施例１と比べ、実際の機械で取得するデータが少なく、手間がかからない効果がある。
【実施例５】
【００１５】
第5の実施例では、解析的に導出された関数が複雑な場合、演算時間の増大やCPU内のレジスタ容量の増加等、コントローラ内での関数計算に負担がかかる。その場合は、以下のように、関数式の近似式を作成することで、計算の負担を軽減できる。
振動周波数を求める式（５）の√（１／（ｌ＋Δｌ）３）をマクローリン展開すると式（７）になる。
ｆ＝ｆ０（１−（３Δｌ／２ｌ）＋（１５Δｌ２／８ｌ２）−（３５Δｌ３／１６ｌ３）＋・・）
・・・（７）  
このとき、アーム長が０．５〜１．５（ｍ）の間で変化するものとし、式(5)に対する式（５）に対する式（７）の誤差が最小になるようにアーム基本長ｌを選択すると、ｌ＝１（ｍ）の時に、誤差が最小になる。式（５）を1〜3次式で近似したものを式（８）、式（９）、式（１０）に示す。
【００１６】
１次近似  
ｆ＝ｆ０（１−３Δｌ／２ｌ  ）                                  ・・・（８）
２次近似
ｆ＝ｆ０（１−（３Δｌ／２ｌ）＋（１５Δｌ２／８ｌ２））)          ・・・（９）
３次近似
ｆ＝ｆ０（１−（３Δｌ／２ｌ）＋（１５Δｌ２／８ｌ２）−（３５Δｌ３／１６ｌ３））  
・・・（１０）
【００１７】
発明の実施例２に関する図2の構成によれば、実施例１〜３と同様にモーション指令解読部３１が移動指令演算部３３へ出力した軸2の移動指令をもとに振動周波数演算部３４が第１軸方向の振動周波数を振動抑制制御部１１の振動周波数パラメータとして設定するので、アーム長が変化しても、振動抑制制御部１１が常に最適動作ができることになるため、常にアームの第１軸方向への振動を抑制できる事に加えて、振動抑制制御の演算をコントローラが行うため、サーボドライバの演算の負担を軽減することができる。
【実施例６】
【００１８】
実施例５で近似式の次数が２以下と低すぎると、計算の負担は小さくなる反面、本来の関数式からの誤差は大きくなってしまう。実施例６では、近似式の各係数に補正係数をかけて誤差の調整を行うことにより、この課題の解決できる。
式(7)，(8)に誤差が小さくなるように補正係数βをかけると次式のようになる。
【００１９】
1次近似
ｆ＝βｆ０（１−３Δｌ／２ｌ  ）                              ・・・（１１）
２次近似
ｆ＝ｆ０（１−β×（３Δｌ／２ｌ）＋（１５Δｌ２／８ｌ２））    ・・・（１２）
このとき、補正係数はパラメータの変動範囲を考慮した値にとればよい。例えば、ｌが０．５〜１．５（ｍ）まで変動するとき、補正係数の値を下記とした場合、最大誤差は下記のように小さくなる。
1次近似の場合β=1.3とすると、54[%]→40[%]
2次近似の場合β=1.25とすると、32[%]→15[%]
第6の実施例によると、１〜２次の近似式で、関数の計算ができるので、通常の部で求めれる関数式をそのまま使用する場合に比べて、平方根演算が不要となるため、計算負荷が非常に低減される。
【産業上の利用可能性】
【００２０】
本発明はロボットが小形化、軽量化されるに伴いアームの剛性が低下するなかでスループットは向上させなければならない市場の要求に対してなされたものであり、姿勢によりアーム先端の振動周波数が変化しても振動をおさえながら高速動作をさせようとしたものである。従ってロボットだけではなく低剛性機械すべてに適用することができる。
【図面の簡単な説明】
【００２１】
【図１】本発明の第1の実施例を示した図
【図２】本発明の第2の実施例を示した図
【図３】本発明の第3の実施例を示した図
【図４】従来例を示した図
【図５】例示している機械システムの概念図
【図６】例示している機械システムのアーム部分のモデル図
【図７】従来例の指令信号
【符号の説明】
【００２２】
１  第１サーボドライバ
２  第２サーボドライバ
３  コントローラ
４  第１モータ
５  第２モータ
６  第１エンコーダ
７  第２エンコーダ
１１  振動抑制制御部
１２，２２  位置制御部
３１  モーション指令解読部
３２，３３  移動指令演算部
３４  振動周波数演算部
【図５】
0004415631.tif 000002
【図６】
0004415631.tif 000003
【図７】
0004415631.tif 000004
【図１】
0004415631.tif 000005
【図２】
0004415631.tif 000006
【図３】
0004415631.tif 000007
【図４】
0004415631.tif 000008