制御装置
【課題】 加速度検出手段の数を増加させずに、駆動軸方向の加速度を直接検出できない駆動軸の加速度をも求めて、被駆動体の振動抑制制御を行う。
【解決手段】 被駆動体３を駆動するＶ軸とＷ軸が直交せず傾斜している。この場合、被駆動体３に直交する複数方向の加速度を検出できる加速度検出手段７を、Ｗ軸方向と該加速度検出手段７で検出する加速度のＹ軸方向を合わせて配置する。Ｖ軸方向の加速度は、加速度検出手段７で検出されるＸ軸、Ｙ軸方向の加速度より演算によって求める。求められたＶ軸、Ｗ軸方向の加速度に基づいて、Ｖ軸、Ｗ軸を駆動するサーボモータへの位置指令、速度指令、または電流指令を補正する。この補正により被駆動体の振動発生を抑制する。複数方向の加速度を検出できる加速度検出手段を１つ設けるだけで、駆動軸方向の加速度を求めることができ、被駆動体が傾ける回転軸を有する場合にも適用できる。
【解決手段】 被駆動体３を駆動するＶ軸とＷ軸が直交せず傾斜している。この場合、被駆動体３に直交する複数方向の加速度を検出できる加速度検出手段７を、Ｗ軸方向と該加速度検出手段７で検出する加速度のＹ軸方向を合わせて配置する。Ｖ軸方向の加速度は、加速度検出手段７で検出されるＸ軸、Ｙ軸方向の加速度より演算によって求める。求められたＶ軸、Ｗ軸方向の加速度に基づいて、Ｖ軸、Ｗ軸を駆動するサーボモータへの位置指令、速度指令、または電流指令を補正する。この補正により被駆動体の振動発生を抑制する。複数方向の加速度を検出できる加速度検出手段を１つ設けるだけで、駆動軸方向の加速度を求めることができ、被駆動体が傾ける回転軸を有する場合にも適用できる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、サーボモータによって被駆動体を駆動し、該被駆動体の位置や速度を制御する制御装置に関する。
【背景技術】
【０００２】
工作機械において、サーボモータで駆動される被駆動体の位置や速度を制御するために、通常、位置フィードバック制御、速度フィードバック制御さらには電流フィードバック制御がなされている。図７はこのサーボモータを制御するサーボ制御部のブロック図である。サーボモータ２または該サーボモータ２によって駆動される被駆動体３には、その速度及び位置を検出する位置検出手段６、速度検出手段５が設けられている。さらには、サーボモータ２を駆動する電流値を検出する電流検出手段４が設けられ、これらの検出手段４，５，６の検出信号がフィードバックされている。
【０００３】
モータ制御部１は、プロセッサで制御するデジタル制御がなされ、位置ループ処理を行う位置制御処理部１１，速度ループ制御を行う速度制御処理部１２，電流ループ処理を行う電流制御処理部１３で構成されている。位置制御処理部１１においては、位置指令と位置検出手段６からの位置フィードバック信号により位置偏差を求め該位置偏差に位置ループゲインを乗じて速度指令が求められる。また、速度制御処理部１２では、位置制御処理部１１から出力された速度指令と速度検出手段５からの速度フィードバック信号により速度偏差を求め比例、積分（または速度偏差の積分に、速度フィードバック信号の比例分を減算）等の速度フィードバック制御を行い電流指令を求める。電流制御処理部１３ではこの電流指令と電流フィードバック信号とにより電流フィードバック制御を行い、サーボアンプを介してサーボモータ２を駆動制御する。
【０００４】
以上が一般的に行われている工作機械の送り軸等の被駆動体３の位置、速度、電流の制御方法であり、通常、プロセッサによってこの位置、速度、電流の制御処理が行われている。このような位置、速度、電流のフィードバック制御を行っても、サーボモータ２の角加速度が急激に変化する際、被駆動体３に振動が発生する場合があり、その対策として、被駆動体３の加速度を検出する加速度センサを設け、該加速度センサからの信号を速度フィードバック制御により出力される電流指令から差し引き、電流フィードバック制御の電流指令とする制御方法が提案されている。
【０００５】
被駆動体に振動が発生すると、加速度センサで検出される被駆動体の加速度信号中の振動成分は、電流フィードバック制御に対する電流指令に対して誤差となることから、この誤差が無くなるように電流指令から差し引き、サーボモータの駆動電流を制御することで振動を抑制したものである（特許文献１参照）。
【０００６】
【特許文献１】特開平６−９１４８２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
直交する２軸または３軸方向の加速度を検出する加速度センサはすでに公知であり、被駆動体を駆動する駆動軸が直交する２軸または３軸であれば、被駆動体に、２軸または３軸方向の加速度を検出する加速度センサを取り付ければ、各駆動軸方向の加速度を検出でき、電流指令等を補正して被駆動体の振動を抑制できる。
【０００８】
しかし、駆動軸が傾斜した傾斜軸を備えた機械の場合、被駆動体に加速度センサを取り付けても、駆動軸方向の加速度を検出できない場合がある。即ち、被駆動体を駆動する駆動軸が直交していない機械の場合、一方の駆動軸に沿った加速度を検出するように加速度センサを被駆動体に配置したとしても、該駆動軸と直交せず傾いた関係にある駆動軸方向の加速度は、加速度センサで検出することができない。検出するには、各駆動軸方向毎にそれぞれの加速度を検出できる加速度センサを配置しなければならず、加速度センサの数が増加するという問題がある。
【０００９】
また、被駆動体を直線移動させる駆動軸と回転揺動させる駆動軸を備える場合、被駆動体に加速度センサを取り付けても、直線駆動軸方向の加速度を検出することはできず、加速度センサを用いた被駆動体の振動抑制方法を適用できないという問題がある。
さらに、傾斜軸等で重力の影響を受ける駆動軸の場合、加速度センサで検出される加速度はすでに重力の影響を受けているものであるから、被駆動体のこの駆動軸方向の加速度を加速度センサで検出したものをそのまま用いることはできないという問題がある。
本発明は、上述した問題を解決することを発明の課題としている。
【課題を解決するための手段】
【００１０】
方向の異なる複数の駆動軸方向に被駆動体をそれぞれ駆動するサーボモータを有し、該被駆動体の少なくとも速度を検出する検出手段を備えて被駆動体の速度を制御する制御装置において、請求項１に係る発明は、複数方向の加速度を検出する加速度検出手段を前記被駆動体に設け、前記加速度検出手段が検出する複数方向の加速度検出値に所定の演算を行うことにより、前記被駆動体を駆動する駆動軸方向の加速度を求め、該加速度演算値により対応する前記駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えるようにした。また、請求項２に係る発明は、直交２軸または直交３軸方向の加速度を検出する加速度検出手段を前記被駆動体に設け、該加速度検出手段で検出される加速度方向のいずれもが駆動軸方向と平行にならない場合、前記加速度検出手段が検出する加速度検出値に所定の演算を行うことにより、駆動軸方向の加速度を求め、前記加速度演算値により対応する前記駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えるようにした。
【００１１】
また、請求項３に係る発明は、被駆動体に設けられ直交２軸または直交３軸方向の加速度を検出する加速度検出手段と、該加速度検出手段の検出方向と前記被駆動体を駆動する駆動軸方向との変位量を検出する変位量検出手段とを設け、前記加速度検出手段が検出する加速度検出値と前記変位検出値とにより所定の演算を行うことにより、駆動軸方向の加速度を求め、求めた加速度演算値により当該駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えるようにした。さらに、請求項４に係る発明は、上述した請求項１乃至３に係る発明において、前記被駆動体は、該被駆動体を揺動させる回転駆動軸を備えたベースに取り付けられているものであって、該ベースを駆動する駆動軸の駆動方向の加速度を前記被駆動体に設けた加速度検出手段で検出した加速度検出値より求め、該ベースを駆動する駆動軸を駆動するサーボモータの前記速度指令または／および電流指令を補正する。さらに、請求項５に係る発明は、前記制御装置が、被駆動体の位置をも制御する制御装置とし、求めた加速度によって前記速度指令または電流指令の代わりに若しくは共に位置指令を補正するようにした。
【００１２】
また、請求項６に係る発明は、被駆動体を方向の異なる複数の駆動軸方向にそれぞれ駆動するサーボモータを有し、該被駆動体の位置および速度を検出する検出手段を備えて被駆動体の位置または、位置および速度を制御する制御装置において、前記被駆動体に設けられ直交２軸または直交３軸方向の加速度を検出する加速度検出手段と、該加速度検出手段の検出方向と駆動軸方向との変位量を指令位置に基づいて推定する変位量推定手段とを備え、前記加速度検出手段が検出する加速度検出値と前記変位推定値とにより所定の演算を行うことにより、被駆動体の進行方向の加速度を求め、前記加速度演算値により当該駆動軸を駆動するサーボモータへの位置指令、速度指令、電流指令の１以上を補正し被駆動体の振動を抑えるようにした。また、請求項７に係る発明は、請求項６に係る発明において、前記被駆動体は、該被駆動体を揺動させる回転駆動軸を備えたベースに取り付けられているものとし、該ベースを駆動する駆動軸の駆動方向の加速度を前記被駆動体に設けた加速度検出手段で検出した加速度検出値より求め、該ベースを駆動する駆動軸を駆動するサーボモータの前記速度指令または／および電流指令を補正するようにした。
また、請求項８に係る発明は、前記加速度検出手段で測定する加速度が重力の影響を受ける場合、駆動軸方向と重力軸方向との変位量から、駆動軸方向の重力成分を演算により求め、重力成分を除去する演算により駆動軸方向の加速度を求める前記補正を行うようにした。
【発明の効果】
【００１３】
本発明は、複数方向の加速度を検出できる加速度検出手段で、被駆動体にかかる駆動軸方向の加速度を直接検出できない場合でも、加速度検出手段を増加することなく、必要とする駆動軸方向の加速度を演算処理によって求め、求められた加速度に基づいて、被駆動体の振動を抑制する制御ができる。駆動軸が直交しない場合にも、被駆動体を傾ける回転軸を有する場合にも、また、重力が影響する駆動軸を有する場合にも、１つの加速度検出器を被駆動体に取り付けることによって、各軸方向の加速度を求めて被駆動体の振動抑制制御ができるものである。
【発明を実施するための最良の形態】
【００１４】
図１は、傾斜軸を有する機械を制御する制御装置に本発明を適用した第１の実施形態の説明図である。
被駆動体３は、Ｖ軸とＷ軸の２つの駆動軸で駆動されるもので、Ｖ軸とＷ軸は直交せず傾いて配設されている。この実施形態では、Ｖ軸の送りねじ９ｖが被駆動体３のベース３ａをＶ軸方向に駆動し、該ベース３ａ上を送りねじ９ｗがＷ軸方向に被駆動体３を駆動するものとしている。そして、被駆動体３には、加速度検出手段７が取り付けられており、該加速度検出手段７は、直交する２軸方向（便宜上、直交するＸ軸、Ｙ軸方向とする）の加速度を検出できるものを用い、該加速度検出手段７が検出する加速度方向のＹ軸方向と駆動軸のＷ軸方向が一致するように該加速度検出手段７は取り付けられているものとしている。即ち、Ｖ軸、Ｗ軸は直交せず、被駆動体に取り付けられた加速度検出手段は直交する方向の加速度しか検出できないことから、駆動軸の一方の方向（図１ではＷ軸）と加速度検出手段７で検出する加速度方向の１つを一致させれば、他方の駆動軸方向と他方の検出加速度方向は異なるものとなる。
【００１５】
そこで、本発明は、加速度検出手段で検出された直交するＸ，Ｙ軸方向の加速度Ａｘ，ＡｙよりＶ軸方向の加速度を演算して求めるものとしている。なお、Ｗ軸方向とＹ軸方向は同一方向であるから、検出したＹ軸方向の加速度ＡｙをＷ軸方向の加速度Ａｗとする。
【００１６】
Ｖ軸方向の加速度Ａｖは、Ｖ軸とＷ軸の交差角度をθとすると、Ｘ，Ｙ軸方向の検出加速度Ａｘ，Ａｙより、次の（１）式の演算を行うことによって求められる。
Ａｖ＝Ａｙ・ｃｏｓθ−Ａｘ・ｓｉｎθ …（１）
このようにして、駆動軸毎に加速度検出手段を配置することなく、各駆動軸方向の加速度を１つの加速度検出手段で検出できるものである。
【００１７】
図２は、この第１の実施形態における、Ｖ軸、Ｗ軸を駆動するサーボモータを制御するモータ制御部のブロック図である。
Ｖ軸、Ｗ軸を駆動するサーボモータのモータ制御部１ｖ，１ｗは図７に示した従来のモータ制御部１と略同一構成であり、相違する点は、検出または演算して求められた駆動方向の加速度に基づいて位置指令、速度指令、電流指令の１つ以上を補正することによって被駆動体の振動を抑制するようにしたものである。
各モータ制御部１ｖ，１ｗは、上位制御装置からの位置指令と被駆動体の駆動方向の位置（またはサーボモータ２ｖ，２ｗの回転位置）を検出する位置検出手段６ｖ，６ｗからの位置フィードバックにより位置偏差を求め速度指令を求める位置制御処理部１１ｖ，１１ｗと、求められた速度指令と被駆動体の駆動方向の速度（またはサーボモータ２ｖ，２ｗの速度）を検出する速度検出手段５ｖ，５ｗからの速度フィードバックにより電流指令を求める速度制御処理部１２ｖ，１２ｗと、該電流指令と、各サーボモータに流れる駆動電流を検出する電流検出手段４ｖ，４ｗからの電流フィードバックにより電流ループ制御処理を行いモータへの駆動電圧指令を出力する電流制御処理部１３ｖ，１３ｗをそれぞれ備え、電流制御処理部１３ｖ，１３ｗから出力される駆動電圧指令に基づいてサーボアンプを介して各サーボモータ２ｖ，２ｗを駆動し、被駆動体３を駆動する。
【００１８】
そして、本実施形態は、さらに、被駆動体３に取り付けられた加速度検出手段７によって検出した直交するＸ軸，Ｙ軸方向の加速度より演算処理部８で、上述した（１）式の演算を行い、Ｖ軸方向の加速度Ａｖを求め、該加速度Ａｖに基づいて、Ｖ軸を駆動するサーボモータ２ｖの位置指令、速度指令、電流指令を補正する。また、Ｗ軸を駆動するサーボモータ２ｗに対しては、加速度検出手段７で検出したＹ軸方向の加速度ＡｙをＷ軸方向の加速度Ａｗとし、該加速度Ａｗに基づいて位置指令、速度指令、電流指令を補正する。
【００１９】
演算して求められたＶ軸方向の加速度Ａｖに係数ａ１を乗じた値を位置指令から減じて補正された位置指令とするか、または、加速度Ａｖに係数ａ２を乗じた値を速度指令から減じて補正された速度指令とするか、加速度Ａｖに係数ａ３を乗じた値を電流指令から減じて補正された電流指令とするか、いずれかの補正を行う。
Ｗ軸に対しても同様にＷ軸方向の検出加速度Ａｗに係数ｂ１を乗じた値を位置指令から減じて補正された位置指令とするか、または、加速度Ａｗに係数ｂ２を乗じた値を速度指令から減じて補正された速度指令とするか、加速度Ａｗに係数ｂ３を乗じた値を電流指令から減じて補正された電流指令とするか、いずれかの補正を行う。
【００２０】
図２では、位置指令、速度指令、電流指令を共に加速度Ａｖに基づいて補正する図を記載しているが、補正するものは、位置指令、速度指令、電流指令のいずれか１つでよい。また、２つ以上補正してもよい。
【００２１】
以上のように、被駆動体３を駆動するＶ軸，Ｗ軸の各駆動軸方向の加速度Ａｖ，Ａｗを求め、該加速度Ａｖ，Ａｗによりそれぞれの駆動軸を駆動するサーボモータ２ｖ，２ｗの位置指令、速度指令、電流指令のいずれか１つ以上を補正するようにしたから、被駆動体の振動を抑制できるものである。加速度が大きいときには、位置指令、速度指令、電流指令は、求められた加速度が大きいときには、大きく減じられ、小さいときは僅かしか減じられないことにより、被駆動体の振動は効率よく抑制される。なお、加速度に基づいて電流指令を補正して被駆動体の振動を抑制する点は、前述した特許文献１に詳細に記載され、加速度に基づいて位置指令、または速度指令を補正して被駆動体の振動を抑制する点は、本願出願人がすでに出願した特願２００４−３４１７７０に詳述しているので、説明を省略する。
【００２２】
上述した第１の実施形態では、被駆動体３を駆動する駆動軸がＶ軸、Ｗ軸の２軸である場合を説明した。被駆動体３を互いに駆動方向が異なる３軸で駆動する場合においては、直交する３軸方向の加速度を検出できる加速度検出手段を用い、直交する２軸の駆動軸があれば、この直交する２軸の駆動軸の方向と加速度検出手段の２つの直交する加速度検出方向を合わせて加速度検出手段を被駆動体に取り付けて残りの駆動軸方向の加速度は、直交する３軸方向の加速度と傾き角度より演算によって求めるようにすればよい。また、３つの駆動軸がいずれも直交しない場合は、１つの駆動軸方向と直交する３軸方向の加速度を検出できる加速度検出手段の加速度検出方向と１つの駆動軸方向を合わせて被駆動体に加速度検出手段を配置し、残りの駆動軸方向の加速度は、その傾き角度と加速度検出手段で検出した３軸方向の加速度に基づいて演算して求めるようにする。
【００２３】
図３は、本発明の第２の実施形態の制御装置が適用される回転軸を有する機械の説明図である。この第２の実施形態は、被駆動体を傾ける回転駆動軸を有する機械の制御装置に本発明を適用した例である。
この第２の実施形態では、被駆動体を工具Ｔが取り付けられる主軸ヘッド３とし該ヘッド３が取り付けられているヘッド３のベース３ａを移動させる駆動軸と、ヘッド３を傾斜させる回転駆動軸を有する例である。図３に示す例では、ヘッド３のベース３ａは直線移動させるＸ軸の駆動軸によりＸ軸方向に駆動され、ヘッド３および該ヘッド３に取り付けられた工具ＴはＸ軸と垂直な軸回りに回転駆動するＣ軸によって、揺動し傾けられるように構成されている。
【００２４】
このような被駆動体を傾ける回転駆動軸を有する場合、被駆動体であるヘッド３に加速度検出手段７を取り付けた場合、該加速度検出手段７で検出される加速度方向は、回転駆動軸のＣ軸の回転量、即ち、ヘッド３の傾き位置によって異なる。また、ヘッド３および工具Ｔを傾かせる回転駆動軸のＣ軸の動作は遅く、該Ｃ軸の駆動によりヘッド３および工具Ｔには振動はほとんど発生せず、無視できる。しかし、ヘッド３のベース３ａを駆動するＸ軸はＣ軸の駆動速度と比較して格段に速く、このＸ軸の駆動がヘッド３および工具Ｔに振動を発生させる要因となる。そのため、Ｘ軸方向の被駆動体（ヘッド３および工具Ｔ）の加速度を求めて、振動が発生しないように、第１の実施形態のように位置指令、速度指令、電流指令を補正するようにする。
【００２５】
図４は、被駆動体のヘッド３および工具ＴがＣ軸の回転により、Ｘ軸方向から回転角θだけ変位した状態におけるＸ軸方向の加速度を求める説明図である。
加速度検出手段７は、ヘッド３に工具軸方向ｂと工具軸方向に対して直交する方向ａの加速度を検出できるように取り付けられている。このとき加速度検出手段７で検出される工具軸方向の加速度をｂ’、工具軸方向に対して直交する方向ａの加速度をａ’とすると、Ｘ軸方向の加速度Ａｘは次の（２）式の演算を行って求めることができる。
【００２６】
Ａｘ＝ａ'ｓｉｎθ＋ｂ'ｃｏｓθ …（２）
回転角θは加速度検出手段７で検出する加速度方向と、駆動軸のＸ軸方向との変位量を示すもので、この回転角θはＣ軸の回転位置、速度を検出する検出器により検出できる。また、Ｃ軸への回転位置指令に基づいて推定する手段を設けることによって求める。
【００２７】
こうして求められた加速度Ａｘに基づいて、Ｘ軸を駆動するサーボモータを駆動制御するモータ制御部での位置指令、速度指令、電流指令を第１の実施形態と同様に補正することによって被駆動体（ヘッド３および工具Ｔ）の振動を抑制することができる。
【００２８】
即ち、この第２の実施形態の場合、図２において、サーボモータ２ｖを、Ｘ軸を駆動するサーボモータとし、演算処理部８での演算を上記（２）式の演算を行うことによってＸ軸方向の加速度Ａｘを求めて、該加速度Ａｘに係数ａ１または係数ａ２または係数ａ３を乗じて、位置指令、速度指令または電流指令の１以上を補正するようにすればよい。また、Ｃ軸に対しては補正を行わないものであるから、Ｃ軸のサーボモータのモータ制御部は、図７に示すような従来のモータ制御部と同じである。
【００２９】
また、ヘッド３のベース３ａを駆動する軸がＸ軸以外にもあるような場合には、その軸方向の加速度をも求めてその軸を駆動するサーボモータへの位置指令、速度指令、電流指令を補正するようにすればよい。例えば、図３，図４に示す例で、Ｘ軸に直交する紙面を貫通する方向（Ｃ軸の軸線方向）にヘッドベース３ａを駆動する軸（例えばＹ軸とする）がある場合には、直交３軸の加速度を検出できる加速度検出器を用い、このＹ軸方向と加速度検出手段で検出する１つの加速度方向が一致するようにすればよい。また、紙面上下方向にヘッドベース３ａを駆動する軸（例えばＺ軸）に有する場合には、Ｃ軸の回転量θによって、該Ｚ軸方向の加速度が変化するから、この場合は、Ｃ軸回転量と各検出加速度に基づいてＺ軸方向の加速度を求めるようにすればよい。
【００３０】
なお、上述した第２の実施形態では、ヘッド３のベース３ａを駆動する駆動軸を直線移動軸の例で示したが、この軸はヘッド３のベース３ａを旋回させる回転駆動軸であっても構わないものである。
【００３１】
以上のように、被駆動体を傾け回転させる駆動軸を備える機械の場合でも、必要とする駆動軸の加速度を複数の方向の加速度を検出できる加速度検出手段を用いて検出でき、この検出した加速度に基づいて、被駆動体の振動を抑制するように補正できるものである。
【００３２】
図５は、本発明の第３の実施形態の制御装置が適用される２つの回転軸を有する機械の説明図である。この第３の実施形態は、工具Ｔが取り付けられるヘッド３に加速度検出手段７が取り付けられ、ヘッド３は、ベース３ａと３ｂによって、回転駆動軸のＣ軸、Ｄ軸によって回転させられる例である。この場合、Ｃ軸とＤ軸の中心を結ぶ線とＸ軸と平行な線との角度をθ、Ｄ軸の中心と工具Ｔの先端とを結ぶ線とＸ軸と平行な線との角度をφとすると、この角度θ、φは、Ｃ軸、Ｄ軸の回転位置を検出する検出器によって検出することができる。また、Ｃ軸、Ｄ軸への位置指令から推定する手段を設けることによって求めることができる。
【００３３】
そして、この角度θ、φと加速度検出手段７で検出される加速度からＸ軸方向の加速度Ａｘを求めることができる。加速度検出手段７で求められる加速度が工具Ｔの軸線方向ｂと該方向と直交する方向ａであり、工具Ｔの軸線方向の加速度がｂ’、該方向と直交する方向ａの加速度がａ’とすると、次の（３）式を演算することによってＸ軸方向の加速度Ａｘを求めることができる。
【００３４】
Ａｘ＝ａ'ｃｏｓ(π−(φ＋θ))＋ｂ'ｃｏｓ((φ＋θ)−π／２)
＝−ａ'ｃｏｓ(φ＋θ)＋ｂ'ｓｉｎ(φ＋θ) …（３）
この第３の実施形態における制御装置は、図２において、サーボモータ２ｖを、Ｘ軸を駆動するサーボモータとし、演算処理部８での演算を上記（３）式の演算を行うことによってＸ軸方向の加速度Ａｘを求めて、該加速度Ａｘに係数ａ１または係数ａ２または係数ａ３を乗じて、位置指令、速度指令または電流指令の１以上を補正するようにすればよい。また、Ｃ軸、Ｄ軸に対しては補正を行わないものであるから、Ｃ軸、Ｄ軸のサーボモータのモータ制御部は、図７に示すような従来のモータ制御部と同じである。
【００３５】
図６は、本発明の第４の実施形態の制御装置が適用される重力の影響を受ける駆動軸と回転軸を有する機械の説明図である。この第４の実施形態は、ヘッド３のベース３ａを駆動する駆動軸のＸ軸が傾斜し、水平ではないことから、該Ｘ軸方向の加速度を加速度検出手段で検出される加速度によって求めるとき、重力の影響を受けることから、重力の影響を排除してＸ軸方向の加速度を求めるようにしたものである。
【００３６】
回転軸Ｃ軸の中心と工具Ｔの先端を結ぶ線と水平軸との角度をθとし、この角度θは、Ｃ軸の回転位置を検出する位置検出手段によって検出するか、またはＣ軸への位置指令に基づいてこの回転角θを推定する手段を設けて求める。また、Ｘ軸の水平面からの傾斜角φは、機械のハードウエア構成が決まると固まる固有の変化のない値であるから、予め制御装置に設定しておけばよい。また、加速度検出手段７をヘッド３に取り付け、該加速度検出手段７で検出する工具Ｔの軸線方向ｂの加速度をｂ’、工具Ｔの軸線方向と直交する方向ａの加速度がａ’、重力をｇとすると、Ｘ軸方向の加速度Ａｘは次の（４）式を演算することによって、重力の影響を排除したｘ軸方向の加速度を求めることができる。
【００３７】
Ａｘ＝−ａ'ｓｉｎ(θ−φ)＋ｂ'ｃｏｓ(θ−φ)-ｇｓｉｎφ …（４）
この第４の実施形態における制御装置は、図２において、サーボモータ２ｖを、Ｘ軸を駆動するサーボモータとし、演算処理部８での演算を上記（４）式の演算を行うことによってＸ軸方向の加速度Ａｘを求めて、該加速度Ａｘに係数ａ１または係数ａ２または係数ａ３を乗じて、位置指令、速度指令または電流指令の１以上を補正することによって、被駆動体のヘッド３，工具Ｔの振動を抑制することができる。また、Ｃ軸に対しては補正を行わないものであるから、Ｃ軸のサーボモータのモータ制御部は、図７に示すような従来のモータ制御部と同じである。以上のように、この第４の実施形態では、複数方向の加速度を検出できる加速度検出手段を用いて、重力の影響を受ける駆動軸の加速度をも重力の影響を排除して検出し、被駆動体に発生する振動を抑制する制御を行うものである。なお、図６に示すように回転軸のＣ軸を有さない機械において、駆動軸が傾き重力の影響を受けるような場合、この第４の実施形態と同様にして、加速度検出器で求めた加速度値より重力の影響を排除した各駆動軸方向の加速度を求めることができるものである。
【００３８】
上述した実施形態では、被駆動体を駆動するサーボモータの制御を位置制御処理部、速度制御処理部、電流制御処理部を有するモータ制御部としたが、位置制御処理部を備えない制御装置にも本発明は適用できるものである。
【図面の簡単な説明】
【００３９】
【図１】本発明の第１の実施形態の制御装置が適用される傾斜軸の説明図である。
【図２】同実施形態における制御装置の要部ブロック図である。
【図３】本発明の第２の実施形態の制御装置が適用される回転軸を有する機械の説明図である。
【図４】同第２の実施形態における回転軸が回転した状態での加速度の説明図である。
【図５】本発明の第３の実施形態の制御装置が適用される２つの回転軸を有する機械の説明図である。
【図６】本発明の第４の実施形態の制御装置が適用される重力の影響を受ける駆動軸と回転軸を有する機械の説明図である。
【図７】従来の制御装置の要部ブロック図である。
【符号の説明】
【００４０】
１、１ｖ、１ｗ モータ制御部
３ 被駆動体
３ａ 被駆動体のベース
７ 加速度検出手段
９ｖ、９ｗ 送りねじ
【技術分野】
【０００１】
本発明は、サーボモータによって被駆動体を駆動し、該被駆動体の位置や速度を制御する制御装置に関する。
【背景技術】
【０００２】
工作機械において、サーボモータで駆動される被駆動体の位置や速度を制御するために、通常、位置フィードバック制御、速度フィードバック制御さらには電流フィードバック制御がなされている。図７はこのサーボモータを制御するサーボ制御部のブロック図である。サーボモータ２または該サーボモータ２によって駆動される被駆動体３には、その速度及び位置を検出する位置検出手段６、速度検出手段５が設けられている。さらには、サーボモータ２を駆動する電流値を検出する電流検出手段４が設けられ、これらの検出手段４，５，６の検出信号がフィードバックされている。
【０００３】
モータ制御部１は、プロセッサで制御するデジタル制御がなされ、位置ループ処理を行う位置制御処理部１１，速度ループ制御を行う速度制御処理部１２，電流ループ処理を行う電流制御処理部１３で構成されている。位置制御処理部１１においては、位置指令と位置検出手段６からの位置フィードバック信号により位置偏差を求め該位置偏差に位置ループゲインを乗じて速度指令が求められる。また、速度制御処理部１２では、位置制御処理部１１から出力された速度指令と速度検出手段５からの速度フィードバック信号により速度偏差を求め比例、積分（または速度偏差の積分に、速度フィードバック信号の比例分を減算）等の速度フィードバック制御を行い電流指令を求める。電流制御処理部１３ではこの電流指令と電流フィードバック信号とにより電流フィードバック制御を行い、サーボアンプを介してサーボモータ２を駆動制御する。
【０００４】
以上が一般的に行われている工作機械の送り軸等の被駆動体３の位置、速度、電流の制御方法であり、通常、プロセッサによってこの位置、速度、電流の制御処理が行われている。このような位置、速度、電流のフィードバック制御を行っても、サーボモータ２の角加速度が急激に変化する際、被駆動体３に振動が発生する場合があり、その対策として、被駆動体３の加速度を検出する加速度センサを設け、該加速度センサからの信号を速度フィードバック制御により出力される電流指令から差し引き、電流フィードバック制御の電流指令とする制御方法が提案されている。
【０００５】
被駆動体に振動が発生すると、加速度センサで検出される被駆動体の加速度信号中の振動成分は、電流フィードバック制御に対する電流指令に対して誤差となることから、この誤差が無くなるように電流指令から差し引き、サーボモータの駆動電流を制御することで振動を抑制したものである（特許文献１参照）。
【０００６】
【特許文献１】特開平６−９１４８２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
直交する２軸または３軸方向の加速度を検出する加速度センサはすでに公知であり、被駆動体を駆動する駆動軸が直交する２軸または３軸であれば、被駆動体に、２軸または３軸方向の加速度を検出する加速度センサを取り付ければ、各駆動軸方向の加速度を検出でき、電流指令等を補正して被駆動体の振動を抑制できる。
【０００８】
しかし、駆動軸が傾斜した傾斜軸を備えた機械の場合、被駆動体に加速度センサを取り付けても、駆動軸方向の加速度を検出できない場合がある。即ち、被駆動体を駆動する駆動軸が直交していない機械の場合、一方の駆動軸に沿った加速度を検出するように加速度センサを被駆動体に配置したとしても、該駆動軸と直交せず傾いた関係にある駆動軸方向の加速度は、加速度センサで検出することができない。検出するには、各駆動軸方向毎にそれぞれの加速度を検出できる加速度センサを配置しなければならず、加速度センサの数が増加するという問題がある。
【０００９】
また、被駆動体を直線移動させる駆動軸と回転揺動させる駆動軸を備える場合、被駆動体に加速度センサを取り付けても、直線駆動軸方向の加速度を検出することはできず、加速度センサを用いた被駆動体の振動抑制方法を適用できないという問題がある。
さらに、傾斜軸等で重力の影響を受ける駆動軸の場合、加速度センサで検出される加速度はすでに重力の影響を受けているものであるから、被駆動体のこの駆動軸方向の加速度を加速度センサで検出したものをそのまま用いることはできないという問題がある。
本発明は、上述した問題を解決することを発明の課題としている。
【課題を解決するための手段】
【００１０】
方向の異なる複数の駆動軸方向に被駆動体をそれぞれ駆動するサーボモータを有し、該被駆動体の少なくとも速度を検出する検出手段を備えて被駆動体の速度を制御する制御装置において、請求項１に係る発明は、複数方向の加速度を検出する加速度検出手段を前記被駆動体に設け、前記加速度検出手段が検出する複数方向の加速度検出値に所定の演算を行うことにより、前記被駆動体を駆動する駆動軸方向の加速度を求め、該加速度演算値により対応する前記駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えるようにした。また、請求項２に係る発明は、直交２軸または直交３軸方向の加速度を検出する加速度検出手段を前記被駆動体に設け、該加速度検出手段で検出される加速度方向のいずれもが駆動軸方向と平行にならない場合、前記加速度検出手段が検出する加速度検出値に所定の演算を行うことにより、駆動軸方向の加速度を求め、前記加速度演算値により対応する前記駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えるようにした。
【００１１】
また、請求項３に係る発明は、被駆動体に設けられ直交２軸または直交３軸方向の加速度を検出する加速度検出手段と、該加速度検出手段の検出方向と前記被駆動体を駆動する駆動軸方向との変位量を検出する変位量検出手段とを設け、前記加速度検出手段が検出する加速度検出値と前記変位検出値とにより所定の演算を行うことにより、駆動軸方向の加速度を求め、求めた加速度演算値により当該駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えるようにした。さらに、請求項４に係る発明は、上述した請求項１乃至３に係る発明において、前記被駆動体は、該被駆動体を揺動させる回転駆動軸を備えたベースに取り付けられているものであって、該ベースを駆動する駆動軸の駆動方向の加速度を前記被駆動体に設けた加速度検出手段で検出した加速度検出値より求め、該ベースを駆動する駆動軸を駆動するサーボモータの前記速度指令または／および電流指令を補正する。さらに、請求項５に係る発明は、前記制御装置が、被駆動体の位置をも制御する制御装置とし、求めた加速度によって前記速度指令または電流指令の代わりに若しくは共に位置指令を補正するようにした。
【００１２】
また、請求項６に係る発明は、被駆動体を方向の異なる複数の駆動軸方向にそれぞれ駆動するサーボモータを有し、該被駆動体の位置および速度を検出する検出手段を備えて被駆動体の位置または、位置および速度を制御する制御装置において、前記被駆動体に設けられ直交２軸または直交３軸方向の加速度を検出する加速度検出手段と、該加速度検出手段の検出方向と駆動軸方向との変位量を指令位置に基づいて推定する変位量推定手段とを備え、前記加速度検出手段が検出する加速度検出値と前記変位推定値とにより所定の演算を行うことにより、被駆動体の進行方向の加速度を求め、前記加速度演算値により当該駆動軸を駆動するサーボモータへの位置指令、速度指令、電流指令の１以上を補正し被駆動体の振動を抑えるようにした。また、請求項７に係る発明は、請求項６に係る発明において、前記被駆動体は、該被駆動体を揺動させる回転駆動軸を備えたベースに取り付けられているものとし、該ベースを駆動する駆動軸の駆動方向の加速度を前記被駆動体に設けた加速度検出手段で検出した加速度検出値より求め、該ベースを駆動する駆動軸を駆動するサーボモータの前記速度指令または／および電流指令を補正するようにした。
また、請求項８に係る発明は、前記加速度検出手段で測定する加速度が重力の影響を受ける場合、駆動軸方向と重力軸方向との変位量から、駆動軸方向の重力成分を演算により求め、重力成分を除去する演算により駆動軸方向の加速度を求める前記補正を行うようにした。
【発明の効果】
【００１３】
本発明は、複数方向の加速度を検出できる加速度検出手段で、被駆動体にかかる駆動軸方向の加速度を直接検出できない場合でも、加速度検出手段を増加することなく、必要とする駆動軸方向の加速度を演算処理によって求め、求められた加速度に基づいて、被駆動体の振動を抑制する制御ができる。駆動軸が直交しない場合にも、被駆動体を傾ける回転軸を有する場合にも、また、重力が影響する駆動軸を有する場合にも、１つの加速度検出器を被駆動体に取り付けることによって、各軸方向の加速度を求めて被駆動体の振動抑制制御ができるものである。
【発明を実施するための最良の形態】
【００１４】
図１は、傾斜軸を有する機械を制御する制御装置に本発明を適用した第１の実施形態の説明図である。
被駆動体３は、Ｖ軸とＷ軸の２つの駆動軸で駆動されるもので、Ｖ軸とＷ軸は直交せず傾いて配設されている。この実施形態では、Ｖ軸の送りねじ９ｖが被駆動体３のベース３ａをＶ軸方向に駆動し、該ベース３ａ上を送りねじ９ｗがＷ軸方向に被駆動体３を駆動するものとしている。そして、被駆動体３には、加速度検出手段７が取り付けられており、該加速度検出手段７は、直交する２軸方向（便宜上、直交するＸ軸、Ｙ軸方向とする）の加速度を検出できるものを用い、該加速度検出手段７が検出する加速度方向のＹ軸方向と駆動軸のＷ軸方向が一致するように該加速度検出手段７は取り付けられているものとしている。即ち、Ｖ軸、Ｗ軸は直交せず、被駆動体に取り付けられた加速度検出手段は直交する方向の加速度しか検出できないことから、駆動軸の一方の方向（図１ではＷ軸）と加速度検出手段７で検出する加速度方向の１つを一致させれば、他方の駆動軸方向と他方の検出加速度方向は異なるものとなる。
【００１５】
そこで、本発明は、加速度検出手段で検出された直交するＸ，Ｙ軸方向の加速度Ａｘ，ＡｙよりＶ軸方向の加速度を演算して求めるものとしている。なお、Ｗ軸方向とＹ軸方向は同一方向であるから、検出したＹ軸方向の加速度ＡｙをＷ軸方向の加速度Ａｗとする。
【００１６】
Ｖ軸方向の加速度Ａｖは、Ｖ軸とＷ軸の交差角度をθとすると、Ｘ，Ｙ軸方向の検出加速度Ａｘ，Ａｙより、次の（１）式の演算を行うことによって求められる。
Ａｖ＝Ａｙ・ｃｏｓθ−Ａｘ・ｓｉｎθ …（１）
このようにして、駆動軸毎に加速度検出手段を配置することなく、各駆動軸方向の加速度を１つの加速度検出手段で検出できるものである。
【００１７】
図２は、この第１の実施形態における、Ｖ軸、Ｗ軸を駆動するサーボモータを制御するモータ制御部のブロック図である。
Ｖ軸、Ｗ軸を駆動するサーボモータのモータ制御部１ｖ，１ｗは図７に示した従来のモータ制御部１と略同一構成であり、相違する点は、検出または演算して求められた駆動方向の加速度に基づいて位置指令、速度指令、電流指令の１つ以上を補正することによって被駆動体の振動を抑制するようにしたものである。
各モータ制御部１ｖ，１ｗは、上位制御装置からの位置指令と被駆動体の駆動方向の位置（またはサーボモータ２ｖ，２ｗの回転位置）を検出する位置検出手段６ｖ，６ｗからの位置フィードバックにより位置偏差を求め速度指令を求める位置制御処理部１１ｖ，１１ｗと、求められた速度指令と被駆動体の駆動方向の速度（またはサーボモータ２ｖ，２ｗの速度）を検出する速度検出手段５ｖ，５ｗからの速度フィードバックにより電流指令を求める速度制御処理部１２ｖ，１２ｗと、該電流指令と、各サーボモータに流れる駆動電流を検出する電流検出手段４ｖ，４ｗからの電流フィードバックにより電流ループ制御処理を行いモータへの駆動電圧指令を出力する電流制御処理部１３ｖ，１３ｗをそれぞれ備え、電流制御処理部１３ｖ，１３ｗから出力される駆動電圧指令に基づいてサーボアンプを介して各サーボモータ２ｖ，２ｗを駆動し、被駆動体３を駆動する。
【００１８】
そして、本実施形態は、さらに、被駆動体３に取り付けられた加速度検出手段７によって検出した直交するＸ軸，Ｙ軸方向の加速度より演算処理部８で、上述した（１）式の演算を行い、Ｖ軸方向の加速度Ａｖを求め、該加速度Ａｖに基づいて、Ｖ軸を駆動するサーボモータ２ｖの位置指令、速度指令、電流指令を補正する。また、Ｗ軸を駆動するサーボモータ２ｗに対しては、加速度検出手段７で検出したＹ軸方向の加速度ＡｙをＷ軸方向の加速度Ａｗとし、該加速度Ａｗに基づいて位置指令、速度指令、電流指令を補正する。
【００１９】
演算して求められたＶ軸方向の加速度Ａｖに係数ａ１を乗じた値を位置指令から減じて補正された位置指令とするか、または、加速度Ａｖに係数ａ２を乗じた値を速度指令から減じて補正された速度指令とするか、加速度Ａｖに係数ａ３を乗じた値を電流指令から減じて補正された電流指令とするか、いずれかの補正を行う。
Ｗ軸に対しても同様にＷ軸方向の検出加速度Ａｗに係数ｂ１を乗じた値を位置指令から減じて補正された位置指令とするか、または、加速度Ａｗに係数ｂ２を乗じた値を速度指令から減じて補正された速度指令とするか、加速度Ａｗに係数ｂ３を乗じた値を電流指令から減じて補正された電流指令とするか、いずれかの補正を行う。
【００２０】
図２では、位置指令、速度指令、電流指令を共に加速度Ａｖに基づいて補正する図を記載しているが、補正するものは、位置指令、速度指令、電流指令のいずれか１つでよい。また、２つ以上補正してもよい。
【００２１】
以上のように、被駆動体３を駆動するＶ軸，Ｗ軸の各駆動軸方向の加速度Ａｖ，Ａｗを求め、該加速度Ａｖ，Ａｗによりそれぞれの駆動軸を駆動するサーボモータ２ｖ，２ｗの位置指令、速度指令、電流指令のいずれか１つ以上を補正するようにしたから、被駆動体の振動を抑制できるものである。加速度が大きいときには、位置指令、速度指令、電流指令は、求められた加速度が大きいときには、大きく減じられ、小さいときは僅かしか減じられないことにより、被駆動体の振動は効率よく抑制される。なお、加速度に基づいて電流指令を補正して被駆動体の振動を抑制する点は、前述した特許文献１に詳細に記載され、加速度に基づいて位置指令、または速度指令を補正して被駆動体の振動を抑制する点は、本願出願人がすでに出願した特願２００４−３４１７７０に詳述しているので、説明を省略する。
【００２２】
上述した第１の実施形態では、被駆動体３を駆動する駆動軸がＶ軸、Ｗ軸の２軸である場合を説明した。被駆動体３を互いに駆動方向が異なる３軸で駆動する場合においては、直交する３軸方向の加速度を検出できる加速度検出手段を用い、直交する２軸の駆動軸があれば、この直交する２軸の駆動軸の方向と加速度検出手段の２つの直交する加速度検出方向を合わせて加速度検出手段を被駆動体に取り付けて残りの駆動軸方向の加速度は、直交する３軸方向の加速度と傾き角度より演算によって求めるようにすればよい。また、３つの駆動軸がいずれも直交しない場合は、１つの駆動軸方向と直交する３軸方向の加速度を検出できる加速度検出手段の加速度検出方向と１つの駆動軸方向を合わせて被駆動体に加速度検出手段を配置し、残りの駆動軸方向の加速度は、その傾き角度と加速度検出手段で検出した３軸方向の加速度に基づいて演算して求めるようにする。
【００２３】
図３は、本発明の第２の実施形態の制御装置が適用される回転軸を有する機械の説明図である。この第２の実施形態は、被駆動体を傾ける回転駆動軸を有する機械の制御装置に本発明を適用した例である。
この第２の実施形態では、被駆動体を工具Ｔが取り付けられる主軸ヘッド３とし該ヘッド３が取り付けられているヘッド３のベース３ａを移動させる駆動軸と、ヘッド３を傾斜させる回転駆動軸を有する例である。図３に示す例では、ヘッド３のベース３ａは直線移動させるＸ軸の駆動軸によりＸ軸方向に駆動され、ヘッド３および該ヘッド３に取り付けられた工具ＴはＸ軸と垂直な軸回りに回転駆動するＣ軸によって、揺動し傾けられるように構成されている。
【００２４】
このような被駆動体を傾ける回転駆動軸を有する場合、被駆動体であるヘッド３に加速度検出手段７を取り付けた場合、該加速度検出手段７で検出される加速度方向は、回転駆動軸のＣ軸の回転量、即ち、ヘッド３の傾き位置によって異なる。また、ヘッド３および工具Ｔを傾かせる回転駆動軸のＣ軸の動作は遅く、該Ｃ軸の駆動によりヘッド３および工具Ｔには振動はほとんど発生せず、無視できる。しかし、ヘッド３のベース３ａを駆動するＸ軸はＣ軸の駆動速度と比較して格段に速く、このＸ軸の駆動がヘッド３および工具Ｔに振動を発生させる要因となる。そのため、Ｘ軸方向の被駆動体（ヘッド３および工具Ｔ）の加速度を求めて、振動が発生しないように、第１の実施形態のように位置指令、速度指令、電流指令を補正するようにする。
【００２５】
図４は、被駆動体のヘッド３および工具ＴがＣ軸の回転により、Ｘ軸方向から回転角θだけ変位した状態におけるＸ軸方向の加速度を求める説明図である。
加速度検出手段７は、ヘッド３に工具軸方向ｂと工具軸方向に対して直交する方向ａの加速度を検出できるように取り付けられている。このとき加速度検出手段７で検出される工具軸方向の加速度をｂ’、工具軸方向に対して直交する方向ａの加速度をａ’とすると、Ｘ軸方向の加速度Ａｘは次の（２）式の演算を行って求めることができる。
【００２６】
Ａｘ＝ａ'ｓｉｎθ＋ｂ'ｃｏｓθ …（２）
回転角θは加速度検出手段７で検出する加速度方向と、駆動軸のＸ軸方向との変位量を示すもので、この回転角θはＣ軸の回転位置、速度を検出する検出器により検出できる。また、Ｃ軸への回転位置指令に基づいて推定する手段を設けることによって求める。
【００２７】
こうして求められた加速度Ａｘに基づいて、Ｘ軸を駆動するサーボモータを駆動制御するモータ制御部での位置指令、速度指令、電流指令を第１の実施形態と同様に補正することによって被駆動体（ヘッド３および工具Ｔ）の振動を抑制することができる。
【００２８】
即ち、この第２の実施形態の場合、図２において、サーボモータ２ｖを、Ｘ軸を駆動するサーボモータとし、演算処理部８での演算を上記（２）式の演算を行うことによってＸ軸方向の加速度Ａｘを求めて、該加速度Ａｘに係数ａ１または係数ａ２または係数ａ３を乗じて、位置指令、速度指令または電流指令の１以上を補正するようにすればよい。また、Ｃ軸に対しては補正を行わないものであるから、Ｃ軸のサーボモータのモータ制御部は、図７に示すような従来のモータ制御部と同じである。
【００２９】
また、ヘッド３のベース３ａを駆動する軸がＸ軸以外にもあるような場合には、その軸方向の加速度をも求めてその軸を駆動するサーボモータへの位置指令、速度指令、電流指令を補正するようにすればよい。例えば、図３，図４に示す例で、Ｘ軸に直交する紙面を貫通する方向（Ｃ軸の軸線方向）にヘッドベース３ａを駆動する軸（例えばＹ軸とする）がある場合には、直交３軸の加速度を検出できる加速度検出器を用い、このＹ軸方向と加速度検出手段で検出する１つの加速度方向が一致するようにすればよい。また、紙面上下方向にヘッドベース３ａを駆動する軸（例えばＺ軸）に有する場合には、Ｃ軸の回転量θによって、該Ｚ軸方向の加速度が変化するから、この場合は、Ｃ軸回転量と各検出加速度に基づいてＺ軸方向の加速度を求めるようにすればよい。
【００３０】
なお、上述した第２の実施形態では、ヘッド３のベース３ａを駆動する駆動軸を直線移動軸の例で示したが、この軸はヘッド３のベース３ａを旋回させる回転駆動軸であっても構わないものである。
【００３１】
以上のように、被駆動体を傾け回転させる駆動軸を備える機械の場合でも、必要とする駆動軸の加速度を複数の方向の加速度を検出できる加速度検出手段を用いて検出でき、この検出した加速度に基づいて、被駆動体の振動を抑制するように補正できるものである。
【００３２】
図５は、本発明の第３の実施形態の制御装置が適用される２つの回転軸を有する機械の説明図である。この第３の実施形態は、工具Ｔが取り付けられるヘッド３に加速度検出手段７が取り付けられ、ヘッド３は、ベース３ａと３ｂによって、回転駆動軸のＣ軸、Ｄ軸によって回転させられる例である。この場合、Ｃ軸とＤ軸の中心を結ぶ線とＸ軸と平行な線との角度をθ、Ｄ軸の中心と工具Ｔの先端とを結ぶ線とＸ軸と平行な線との角度をφとすると、この角度θ、φは、Ｃ軸、Ｄ軸の回転位置を検出する検出器によって検出することができる。また、Ｃ軸、Ｄ軸への位置指令から推定する手段を設けることによって求めることができる。
【００３３】
そして、この角度θ、φと加速度検出手段７で検出される加速度からＸ軸方向の加速度Ａｘを求めることができる。加速度検出手段７で求められる加速度が工具Ｔの軸線方向ｂと該方向と直交する方向ａであり、工具Ｔの軸線方向の加速度がｂ’、該方向と直交する方向ａの加速度がａ’とすると、次の（３）式を演算することによってＸ軸方向の加速度Ａｘを求めることができる。
【００３４】
Ａｘ＝ａ'ｃｏｓ(π−(φ＋θ))＋ｂ'ｃｏｓ((φ＋θ)−π／２)
＝−ａ'ｃｏｓ(φ＋θ)＋ｂ'ｓｉｎ(φ＋θ) …（３）
この第３の実施形態における制御装置は、図２において、サーボモータ２ｖを、Ｘ軸を駆動するサーボモータとし、演算処理部８での演算を上記（３）式の演算を行うことによってＸ軸方向の加速度Ａｘを求めて、該加速度Ａｘに係数ａ１または係数ａ２または係数ａ３を乗じて、位置指令、速度指令または電流指令の１以上を補正するようにすればよい。また、Ｃ軸、Ｄ軸に対しては補正を行わないものであるから、Ｃ軸、Ｄ軸のサーボモータのモータ制御部は、図７に示すような従来のモータ制御部と同じである。
【００３５】
図６は、本発明の第４の実施形態の制御装置が適用される重力の影響を受ける駆動軸と回転軸を有する機械の説明図である。この第４の実施形態は、ヘッド３のベース３ａを駆動する駆動軸のＸ軸が傾斜し、水平ではないことから、該Ｘ軸方向の加速度を加速度検出手段で検出される加速度によって求めるとき、重力の影響を受けることから、重力の影響を排除してＸ軸方向の加速度を求めるようにしたものである。
【００３６】
回転軸Ｃ軸の中心と工具Ｔの先端を結ぶ線と水平軸との角度をθとし、この角度θは、Ｃ軸の回転位置を検出する位置検出手段によって検出するか、またはＣ軸への位置指令に基づいてこの回転角θを推定する手段を設けて求める。また、Ｘ軸の水平面からの傾斜角φは、機械のハードウエア構成が決まると固まる固有の変化のない値であるから、予め制御装置に設定しておけばよい。また、加速度検出手段７をヘッド３に取り付け、該加速度検出手段７で検出する工具Ｔの軸線方向ｂの加速度をｂ’、工具Ｔの軸線方向と直交する方向ａの加速度がａ’、重力をｇとすると、Ｘ軸方向の加速度Ａｘは次の（４）式を演算することによって、重力の影響を排除したｘ軸方向の加速度を求めることができる。
【００３７】
Ａｘ＝−ａ'ｓｉｎ(θ−φ)＋ｂ'ｃｏｓ(θ−φ)-ｇｓｉｎφ …（４）
この第４の実施形態における制御装置は、図２において、サーボモータ２ｖを、Ｘ軸を駆動するサーボモータとし、演算処理部８での演算を上記（４）式の演算を行うことによってＸ軸方向の加速度Ａｘを求めて、該加速度Ａｘに係数ａ１または係数ａ２または係数ａ３を乗じて、位置指令、速度指令または電流指令の１以上を補正することによって、被駆動体のヘッド３，工具Ｔの振動を抑制することができる。また、Ｃ軸に対しては補正を行わないものであるから、Ｃ軸のサーボモータのモータ制御部は、図７に示すような従来のモータ制御部と同じである。以上のように、この第４の実施形態では、複数方向の加速度を検出できる加速度検出手段を用いて、重力の影響を受ける駆動軸の加速度をも重力の影響を排除して検出し、被駆動体に発生する振動を抑制する制御を行うものである。なお、図６に示すように回転軸のＣ軸を有さない機械において、駆動軸が傾き重力の影響を受けるような場合、この第４の実施形態と同様にして、加速度検出器で求めた加速度値より重力の影響を排除した各駆動軸方向の加速度を求めることができるものである。
【００３８】
上述した実施形態では、被駆動体を駆動するサーボモータの制御を位置制御処理部、速度制御処理部、電流制御処理部を有するモータ制御部としたが、位置制御処理部を備えない制御装置にも本発明は適用できるものである。
【図面の簡単な説明】
【００３９】
【図１】本発明の第１の実施形態の制御装置が適用される傾斜軸の説明図である。
【図２】同実施形態における制御装置の要部ブロック図である。
【図３】本発明の第２の実施形態の制御装置が適用される回転軸を有する機械の説明図である。
【図４】同第２の実施形態における回転軸が回転した状態での加速度の説明図である。
【図５】本発明の第３の実施形態の制御装置が適用される２つの回転軸を有する機械の説明図である。
【図６】本発明の第４の実施形態の制御装置が適用される重力の影響を受ける駆動軸と回転軸を有する機械の説明図である。
【図７】従来の制御装置の要部ブロック図である。
【符号の説明】
【００４０】
１、１ｖ、１ｗ モータ制御部
３ 被駆動体
３ａ 被駆動体のベース
７ 加速度検出手段
９ｖ、９ｗ 送りねじ
【特許請求の範囲】
【請求項１】
方向の異なる複数の駆動軸方向に被駆動体をそれぞれ駆動するサーボモータを有し、該被駆動体の少なくとも速度を検出する検出手段を備えて被駆動体の速度を制御する制御装置において、複数方向の加速度を検出する加速度検出手段を前記被駆動体に設け、前記加速度検出手段が検出する複数方向の加速度検出値に所定の演算を行うことにより、前記被駆動体を駆動する駆動軸方向の加速度を求め、該加速度演算値により対応する前記駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えることを特徴とする制御装置。
【請求項２】
方向の異なる複数の駆動軸方向に被駆動体をそれぞれ駆動するサーボモータを有し、該被駆動体の少なくとも速度を検出する検出手段を備えて被駆動体の速度を制御する制御装置において、直交２軸または直交３軸方向の加速度を検出する加速度検出手段を前記被駆動体に設け、該加速度検出手段で検出される加速度方向のいずれもが駆動軸方向と平行にならない場合、前記加速度検出手段が検出する加速度検出値に所定の演算を行うことにより、駆動軸方向の加速度を求め、前記加速度演算値により対応する前記駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えることを特徴とする制御装置。
【請求項３】
方向の異なる複数の駆動軸方向に被駆動体をそれぞれ駆動するサーボモータを有し、該被駆動体の少なくとも速度を検出する検出手段を備えて被駆動体の速度を制御する制御装置において、前記被駆動体に設けられ直交２軸または直交３軸方向の加速度を検出する加速度検出手段と、該加速度検出手段の検出方向と前記被駆動体を駆動する駆動軸方向との変位量を検出する変位量検出手段とを設け、前記加速度検出手段が検出する加速度検出値と前記変位検出値とにより所定の演算を行うことにより、駆動軸方向の加速度を求め、求めた加速度演算値により当該駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えることを特徴とした制御装置。
【請求項４】
前記被駆動体は、該被駆動体を揺動させる回転駆動軸を備えたベースに取り付けられ、該ベースを駆動する駆動軸の駆動方向の加速度を前記被駆動体に設けた加速度検出手段で検出した加速度検出値より求め、該ベースを駆動する駆動軸を駆動するサーボモータの前記速度指令または／および電流指令を補正する請求項１乃至３の内いずれか１項に記載の制御装置。
【請求項５】
前記制御装置は、被駆動体の位置をも制御する制御装置であって、求めた加速度によって前記速度指令または電流指令の代わりに若しくは共に位置指令を補正する請求項１乃至４の内いずれか１項に記載の制御装置。
【請求項６】
被駆動体を方向の異なる複数の駆動軸方向にそれぞれ駆動するサーボモータを有し、該被駆動体の位置および速度を検出する検出手段を備えて被駆動体の位置または、位置および速度を制御する制御装置において、前記被駆動体に設けられ直交２軸または直交３軸方向の加速度を検出する加速度検出手段と、該加速度検出手段の検出方向と駆動軸方向との変位量を指令位置に基づいて推定する変位量推定手段とを備え、前記加速度検出手段が検出する加速度検出値と前記変位推定値とにより所定の演算を行うことにより、被駆動体の進行方向の加速度を求め、前記加速度演算値により当該駆動軸を駆動するサーボモータの位置指令、速度指令、電流指令の１以上を補正し被駆動体の振動を抑えることを特徴とする制御装置。
【請求項７】
前記被駆動体は、該被駆動体を揺動させる回転駆動軸を備えたベースに取り付けられ、該ベースを駆動する駆動軸の駆動方向の加速度を前記被駆動体に設けた加速度検出手段で検出した加速度検出値より求め、該ベースを駆動する駆動軸を駆動するサーボモータの位置指令、速度指令、電流指令の１以上を補正する請求項６に記載の制御装置。
【請求項８】
前記加速度検出手段で測定する加速度が重力の影響を受ける場合、駆動軸方向と重力軸方向との変位量から、駆動軸方向の重力成分を演算により求め、重力成分を除去する演算により駆動軸方向の加速度を求め、前記加速度演算値により前記補正を行う請求項１乃至７の内いずれか１項に記載の制御装置。
【請求項１】
方向の異なる複数の駆動軸方向に被駆動体をそれぞれ駆動するサーボモータを有し、該被駆動体の少なくとも速度を検出する検出手段を備えて被駆動体の速度を制御する制御装置において、複数方向の加速度を検出する加速度検出手段を前記被駆動体に設け、前記加速度検出手段が検出する複数方向の加速度検出値に所定の演算を行うことにより、前記被駆動体を駆動する駆動軸方向の加速度を求め、該加速度演算値により対応する前記駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えることを特徴とする制御装置。
【請求項２】
方向の異なる複数の駆動軸方向に被駆動体をそれぞれ駆動するサーボモータを有し、該被駆動体の少なくとも速度を検出する検出手段を備えて被駆動体の速度を制御する制御装置において、直交２軸または直交３軸方向の加速度を検出する加速度検出手段を前記被駆動体に設け、該加速度検出手段で検出される加速度方向のいずれもが駆動軸方向と平行にならない場合、前記加速度検出手段が検出する加速度検出値に所定の演算を行うことにより、駆動軸方向の加速度を求め、前記加速度演算値により対応する前記駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えることを特徴とする制御装置。
【請求項３】
方向の異なる複数の駆動軸方向に被駆動体をそれぞれ駆動するサーボモータを有し、該被駆動体の少なくとも速度を検出する検出手段を備えて被駆動体の速度を制御する制御装置において、前記被駆動体に設けられ直交２軸または直交３軸方向の加速度を検出する加速度検出手段と、該加速度検出手段の検出方向と前記被駆動体を駆動する駆動軸方向との変位量を検出する変位量検出手段とを設け、前記加速度検出手段が検出する加速度検出値と前記変位検出値とにより所定の演算を行うことにより、駆動軸方向の加速度を求め、求めた加速度演算値により当該駆動軸を駆動するサーボモータの速度指令または／および電流指令を補正し被駆動体の振動を抑えることを特徴とした制御装置。
【請求項４】
前記被駆動体は、該被駆動体を揺動させる回転駆動軸を備えたベースに取り付けられ、該ベースを駆動する駆動軸の駆動方向の加速度を前記被駆動体に設けた加速度検出手段で検出した加速度検出値より求め、該ベースを駆動する駆動軸を駆動するサーボモータの前記速度指令または／および電流指令を補正する請求項１乃至３の内いずれか１項に記載の制御装置。
【請求項５】
前記制御装置は、被駆動体の位置をも制御する制御装置であって、求めた加速度によって前記速度指令または電流指令の代わりに若しくは共に位置指令を補正する請求項１乃至４の内いずれか１項に記載の制御装置。
【請求項６】
被駆動体を方向の異なる複数の駆動軸方向にそれぞれ駆動するサーボモータを有し、該被駆動体の位置および速度を検出する検出手段を備えて被駆動体の位置または、位置および速度を制御する制御装置において、前記被駆動体に設けられ直交２軸または直交３軸方向の加速度を検出する加速度検出手段と、該加速度検出手段の検出方向と駆動軸方向との変位量を指令位置に基づいて推定する変位量推定手段とを備え、前記加速度検出手段が検出する加速度検出値と前記変位推定値とにより所定の演算を行うことにより、被駆動体の進行方向の加速度を求め、前記加速度演算値により当該駆動軸を駆動するサーボモータの位置指令、速度指令、電流指令の１以上を補正し被駆動体の振動を抑えることを特徴とする制御装置。
【請求項７】
前記被駆動体は、該被駆動体を揺動させる回転駆動軸を備えたベースに取り付けられ、該ベースを駆動する駆動軸の駆動方向の加速度を前記被駆動体に設けた加速度検出手段で検出した加速度検出値より求め、該ベースを駆動する駆動軸を駆動するサーボモータの位置指令、速度指令、電流指令の１以上を補正する請求項６に記載の制御装置。
【請求項８】
前記加速度検出手段で測定する加速度が重力の影響を受ける場合、駆動軸方向と重力軸方向との変位量から、駆動軸方向の重力成分を演算により求め、重力成分を除去する演算により駆動軸方向の加速度を求め、前記加速度演算値により前記補正を行う請求項１乃至７の内いずれか１項に記載の制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２００６−１５９３４５（Ｐ２００６−１５９３４５Ａ）
【公開日】平成１８年６月２２日（２００６．６．２２）
【国際特許分類】
【出願番号】特願２００４−３５４３８９（Ｐ２００４−３５４３８９）
【出願日】平成１６年１２月７日（２００４．１２．７）
【出願人】（３９０００８２３５）ファナック株式会社 (1,110)
【Ｆターム（参考）】
【公開日】平成１８年６月２２日（２００６．６．２２）
【国際特許分類】
【出願日】平成１６年１２月７日（２００４．１２．７）
【出願人】（３９０００８２３５）ファナック株式会社 (1,110)
【Ｆターム（参考）】
[ Back to top ]