(57)【要約】      （修正有）【課題】状態で周波数が変化する低剛性機械の振動を抑制する制御装置を提供する。【解決手段】モーション指令解読手段３１で計算した第１軸と第２軸の移動量をそれぞれ第１軸の第１移動指令演算手段３２と第２軸の第２移動指令演算手段３３に出力し、第１移動指令出力手段３１は第２移動指令出力手段３２はそれぞれ第１サーボドライバ１と第２サーボドライバ２へ移動指令を出力するコントローラ３とで構成され、コントローラ３の振動周波数演算手段３４は第２移動指令手段３３にもとづき、第１軸の振動周波数を逐次演算し、第１サーボドライバ１の振動抑制手段１１に出力するように構成したので状態により振動周波数が変化する機械に対しても常に制振効果を得ることができる。【選択図】図１
2005103739.tif 000002
【特許請求の範囲】
【請求項１】
第１軸を駆動する第１モータと、前記第１モータに取り付けられた第１エンコーダの位置信号をフィードバックし前記第１モータを制御する第１位置制御手段と振動抑制制御手段とからなる第１サーボドライバと、
第２軸を駆動する第２モータと、前記第２モータに取り付けられた第２エンコーダの位置信号をフィードバックし前記第２モータを制御する第２位置制御手段からなる第２サーボドライバと、
モーション指令解読手段で計算した前記第１軸と前記第２軸の移動量をそれぞれ第１移動指令演算手段と第２移動指令演算手段に出力し、前記第１移動指令出力手段と前記第２移動指令出力手段はそれぞれ前記第１サーボドライバと前記第２サーボドライバへ移動指令を出力するコントローラとで構成され、
前記コントローラは前記第２移動指令手段の出力に基づき、振動周波数演算手段により前記第１軸の振動周波数を逐次演算し、前記第１サーボドライバの前記振動抑制手段に出力することを特徴とする振動抑制制御装置。
【請求項２】
前記振動抑制制御手段を前記第１サーボドライバから前記コントローラに移設し、前記コントローラの前記振動周波数抑制手段は前記第２軸の移動指令を基に第１軸の振動周波数パラメータを演算し、前記振動抑制制御手段に設定することを特徴とする請求項１記載の振動抑制制御装置。
【請求項３】
前記振動周波数演算手段を前記コントローラから前記第１サーボドライバへ移設し、第２軸の位置情報を通信手段により前記第１サーボドライバの前記周波数演算手段に伝送し、前記振動周波数演算手段は前記第２軸の位置情報を基に前記第１軸の振動周波数を演算し、前記振動抑制制御手段に振動周波数パラメータとして逐次設定することを特徴とした請求項１記載の振動抑制制御装置。
【請求項４】
第２サーボドライバに設けた複数の出力接点と第１サーボドライバに設けた複数の入力接点とを接続し、前記出力接点は第２軸の位置情報に基づいてON,OFF情報を出力し、前記第１サーボドライバは、前記ON,OFF情報から、前記第２サーボドライバの第２軸の位置情報を接点間範囲内として特定し、特定した位置情報に従って、第１軸の振動周波数を演算し、前記振動抑制手段の前記振動周波数パラメータとして逐次設定することを特徴とする請求項１記載の振動抑制制御装置。
【請求項５】
振動周波数演算を関数で行うことを特徴とする請求項１乃至３記載の振動抑制制御装置。
【請求項６】
前記関数を近似式により構成した請求項５記載の振動抑制制御装置。
【請求項７】
前記近似式を最大次数２として、誤差が大きい場合、近似式の各係数に補正係数をかけて誤差の調整を行う請求項６記載の振動抑制制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は低剛性機械の振動抑制装置に関する。
【背景技術】
【０００２】
従来、低剛性機械の振動抑制方法には例えば特許文献１がある。
図５により従来技術を説明する。図5で示されるように、機械は第１サーボドライバと第１モータからなる第１軸と第２サーボドライバと第２モータからなる第２軸で構成される。この機械の動作は、第２軸によってアームが伸縮し、第１軸によってアームが前後する。アームの前後によってアームは加振され、アームの伸縮によってアーム先端の振動周波数が変化する。この振動は、アーム先端からモータ間の機構によって吸収されてしまうため、モータを介してフィードバック信号を得ているサーボドライバには、振動していることが検出できない。しかし、この機械は、アーム先端が仕事をするので、アーム先端の振動が問題になる。この場合、図4のように、第１軸を駆動する第１サーボドライバ内で、あるアームの状態における振動周波数に対して設定した振動抑制制御を使用する必要がある。
また、特許文献１の従来技術では指令器から払い出される指令を通常の台形指令や三角波指令ではなく、アームの振動が発生しづらい指令を払い出す方法が開示されている。図７において、上は速度指令信号であり、加速時間の７５％の時刻で加速度を最大とする。さらに減速時間の２５％の時刻で減速の加速度を最大とする。つまり、アームの加速開始すなわち起動から加速終了すなわち所定速度に達するまでの加速時間において、加速開始時刻から起算して、加速時間の７５％または約７５％の時間が経過した時刻に、加速度のピークが来るよう、アーム駆動手段への加速指令を行う。また、アームの減速開始から減速終了すなわち停止までの減速時間においては、減速開始時刻から起算して、減速時間の２５％または約２５％の時間が経過した時刻に、加速度の負のピークが来るように、アーム駆動手段への減速指令を行うものである。
図5のような機械では、例えば１０００ｍｍから１５００ｍｍへアームが伸びると、振動周波数は２０Ｈｚから１１Ｈｚへと2倍程度変化する。サーボドライバによる従来の振動抑制制御は、設定した特定の周波数に対して振動をダンピングするものである。許容している振動周波数のずれは１０％〜２０％程度でしかない。
【０００３】
【特許文献１】特開２０００−７９５８３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、従来の振動抑制制御はもっとも振幅が大きい状態に対してのみ振動抑制を考慮しているため、図５のように振動周波数が変化する機械に対しては、常には制振効果を得ることができない。
また特許文献１のように指令を変形して振動抑制効果を得る方法でも、図5のように振動周波数が変化する機械に対しては、常には制振効果を得ることができない。
本発明は、上記問題を解決するためになされたものであり、従来技術では、サーボドライバのみで実行していた振動抑制制御の設定を、コントローラと連携することにより、振動周波数が変化する機械に対しても常に制振効果を得ることを目的とする。
【課題を解決するための手段】
【０００５】
請求項１記載の本発明は、第１軸を駆動する第１モータと、第１モータに取り付けられた第１エンコーダの位置信号をフィードバックし第１モータを制御する第１位置制御手段と振動抑制制御手段とからなる第１サーボドライバと、第２軸を駆動する第２モータと、第２モータに取り付けられた第２エンコーダの位置信号をフィードバックし第２モータを制御する第２位置制御手段からなる第２サーボドライバと、モーション指令解読手段で計算した第１軸と第２軸の移動量をそれぞれ第１移動指令演算手段と第２移動指令演算手段に出力し、第１移動指令出力手段と第２移動指令出力手段はそれぞれ第１サーボドライバと第２サーボドライバへ移動指令を出力するコントローラとで構成され、コントローラは第２移動指令手段の出力にもとづき、第１軸の振動周波数を逐次演算し、第１サーボドライバの振動抑制手段に出力するように構成したので状態により振動周波数が変化する機械に対しても常に制振効果を得ることができる。
請求項２記載の本発明は、請求項１記載の振動抑制制御装置において、請求項１の振動抑制制御手段を第１サーボドライバからコントローラに移設し、コントローラの振動周波数抑制手段は第２軸の移動指令をもとに第１軸の振動周波数パラメータを演算し振動抑制制御手段に設定するように構成したので状態により振動周波数が変化する機械に対しても常に制振効果を得ることができる。
請求項3記載の本発明は、請求項１記載の振動抑制制御装置において、請求項１の振動周波数演算手段をコントローラから第１サーボドライバへ移設し、第２軸の位置情報を通信手段により第１サーボドライバの周波数演算手段に伝送し、振動周波数演算手段は第２軸の位置情報をもとに第１軸の振動周波数を演算し、振動抑制制御手段に振動周波数パラメータとして逐次設定するように構成したので状態により振動周波数が変化する機械に対しても常に制振効果を得ることができる。
請求項４記載の本発明は、請求項１記載の振動抑制制御装置において、第２サーボドライバに設けた複数の出力接点と第１サーボドライバに設けた複数の入力接点とを接続し、出力接点は第２軸の位置情報に基づいてON,OFF情報を出力し、第１サーボドライバは、ON,OFF情報から、第２サーボドライバの第２軸の位置情報をある接点間に特定し、特定した位置情報に従って、第１軸の振動周波数を演算し、振動抑制手段の振動周波数パラメータとして逐次設定するように構成したので状態により振動周波数が変化する機械に対しても常に制振効果を得ることができる。
請求項５記載の本発明は、請求項１記載の振動抑制制御装置において、振動周波数演算を関数で行うように構成したので状態により振動周波数が変化する機械に対しても常に制振効果を得ることができる。
請求項６記載の本発明は、請求項５記載の振動抑制制御装置において、関数を近似式により構成したので状態により振動周波数が変化する機械に対しても常に制振効果を得ることができる。
請求項７記載の本発明は、請求項６記載の振動抑制制御装置において、近似式を最大次数２として、誤差が大きい場合、近似式の各係数に補正係数をかけて誤差の調整を行う請ように構成したので状態により振動周波数が変化する機械に対しても常に制振効果を得ることができる。
【発明の効果】
【０００６】
本発明は、振動抑制制御の設定を、コントローラと連携することにより、振動周波数が変化する機械に対しても常に制振効果を得られる効果がある。
【発明を実施するための最良の形態】
【０００７】
以下、本発明の実施例を図を用いて説明する。
【実施例１】
【０００８】
図１は実施例１の構成図である。図1において、１は第１サーボドライバ、２は第２サーボドライバ、３はコントローラ、４は第1モータ、５は第２モータ、６は第１エンコータ、７は第２エンコーダである。また、３１はモーション指令解読演算手段、３２は第１移動指令演算手段、３３は第２移動指令演算手段である。さらに、１１は振動抑制制御手段、１２は第１位置制御手段、２２は第２位置制御手段である。次に動作について説明する。
コントローラの第1軸の第１移動指令演算手段３２と第２軸の第２移動指令演算手段３３は、モーション指令解読手段３１で計算した第１軸と第２軸への移動量をもとに第１サーボドライバおよび第２サーボドライバへの移動指令を出力する。
ここで、図5に示すロボットアームでは、第１軸方向の振動の周波数はアーム長で変わることが分かっており、アーム長は第２軸の位置によって一意に決まるので、第２軸の位置に応じて、アームの振動周波数を出力する振動周波数演算手段３４を新たにモーションコントローラ内に追加する。振動周波数演算手段３４が出力する振動周波数パラメータ値を第１サーボドライバの振動抑制制御手段１１に入力する。コントローラと第１サーボドライバ間をシリアル通信等のデータ伝送手段で接続する場合は、シリアル通信での通常のパラメータ伝送方式にて、容易に振動周波数を振動抑制制御手段１１に設定することができる。また、パルス列信号により移動距離を伝送する場合は、信号伝送路とは別にサーボドライバの外部接点信号をコントローラから駆動してサーボ内部の振動周波数パラメータを変更することにより実現できる。また、振動周波数演算手段１１は、例えば、第２軸の移動量とアームの振動周波数との関係をあらかじめ計測してテーブルを作成し、入力した第２軸の移動量からメモリテーブルを引いて周波数を出力する方式で構成する。モーション指令解読手段３１が移動指令演算手段３３へ出力した第２軸の移動指令をもとに振動周波数演算手段３４が第１軸方向の振動周波数を振動抑制制御手段１１の振動周波数パラメータとして設定するので、アーム長が変化しても、振動抑制制御手段１１が常に最適な動作をするため、常にアームの軸1方向への振動を抑制できる。
【実施例２】
【０００９】
次に本発明の実施例２について説明する。実施例２を図２で示す。図1との相違点は、図1では、第１サーボドライバのなかで構成した振動抑制制御１１をコントローラで実現している点である。コントローラのなかでモーション指令解読手段３１が移動指令演算手段３３へ出力した第２軸の移動指令をもとに振動周波数演算手段３４が第１軸方向の振動周波数を振動抑制制御手段１１の振動周波数パラメータとして設定するので、アーム長が変化しても、振動抑制制御手段１１が常に最適動作ができることになるため、常にアームの第１軸方向の振動を抑制できる。また、振動抑制制御手段１１をコントローラ内で構成しているので、振動周波数パラメータを第１軸へ伝送する必要がなく、第１サーボドライバは第２サーボドライバと同様な通常のサーボドライバでよいという効果がある。
【実施例３】
【００１０】
次に本発明の実施例３について説明する。実施例３を図3に示す。図1との相違点は、図1では、コントローラ内に構成した振動周波数演算手段３４を第１サーボドライバ内で実現している点と第２サーボドライバの位置情報を第１サーボドライバへ通信手段４１にて伝送している点である。第１サーボドライバ内では、前記通信手段４１にて伝送された第２軸の位置情報を振動周波数演算手段３４に入力して、第１軸の振動周波数を演算し、前記振動周波数演算手段１１の振動周波数パラメータとして逐次設定する。なお、通信手段４１はシリアル通信で実現できるし、パラレル信号で伝送することもできる。また、第２サーボドライバの出力接点と第１サーボドライバの入力接点とを接続し、第２サーボドライバ内の第２軸の位置情報に従った接点のON,OFF情報を出力する。第１サーボドライバは、ON,OFF情報から、第２サーボドライバ内の第２軸の位置情報を復元することができる。復元した位置情報に従って、第１軸の振動周波数を演算し、振動周波数演算手段１１の振動周波数パラメータとして逐次設定することもできる。
【実施例４】
【００１１】
次に、本発明の実施例４を説明する。本発明の第１〜３の実施例では、振動周波数演算手段１１のテーブルを作成するために実際の機械で取得すべきデータ点数が多く、作業量が増えてしまう。この作業量を軽減するため、第４の実施例では関数演算にて振動周波数演算手段１１を構成する。なお、前記第１〜３の実施例ではすべて、振動周波数演算手段３４を用いるので、下記の他の周波数演算方法はすべて前記第１〜３の実施例の制御系の構成にあてはまるので、以下第1の実施例の構成で代表して説明する。
下記のように、第1軸および第2軸の機械モデルを作成する。図5のような機械系を考える。このとき、第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の実施例を示した図
【図４】従来例を示した図
【図５】例示している機械システムの概念図
【図６】例示している機械システムのアーム部分のモデル図
【図７】従来例の指令信号
【符号の説明】
【００２２】
１  第１サーボドライバ
２  第２サーボドライバ
３  コントローラ
４  第１モータ
５  第２モータ
６  第１エンコーダ
７  第２エンコーダ
１１  振動抑制制御手段
１２，２２  位置制御手段
３１  モーション指令解読手段
３２，３３  移動指令演算手段
３４  振動周波数演算手段
【図１】
2005103739.tif 000003
【図２】
2005103739.tif 000004
【図３】
2005103739.tif 000005
【図４】
2005103739.tif 000006
【図５】
2005103739.tif 000007
【図６】
2005103739.tif 000008
【図７】
2005103739.tif 000009